diff --git a/Examples/.svn/all-wcprops b/Examples/.svn/all-wcprops new file mode 100644 index 0000000000000000000000000000000000000000..7812909069b480909bd0c08a3b92d3d68ee71333 --- /dev/null +++ b/Examples/.svn/all-wcprops @@ -0,0 +1,5 @@ +K 25 +svn:wc:ra_dav:version-url +V 45 +/svn/g_rect_package/!svn/ver/7/trunk/Examples +END diff --git a/Examples/.svn/entries b/Examples/.svn/entries new file mode 100644 index 0000000000000000000000000000000000000000..8bb71970ab4c7ca9e2ec64b1eb38a27277b1e162 --- /dev/null +++ b/Examples/.svn/entries @@ -0,0 +1,34 @@ +10 + +dir +7 +http://bourda02@demeter/svn/g_rect_package/trunk/Examples +http://bourda02@demeter/svn/g_rect_package + + + +2013-04-15T22:39:22.457529Z +7 +bourda02 + + + + + + + + + + + + + + +011fb189-60de-4f9a-bef2-e507afd36f24 + +Field +dir + +Lab +dir + diff --git a/Examples/Field/.svn/all-wcprops b/Examples/Field/.svn/all-wcprops new file mode 100644 index 0000000000000000000000000000000000000000..c71f1d49c2359130b44b6a6dfba1174c4af9a74e --- /dev/null +++ b/Examples/Field/.svn/all-wcprops @@ -0,0 +1,17 @@ +K 25 +svn:wc:ra_dav:version-url +V 51 +/svn/g_rect_package/!svn/ver/7/trunk/Examples/Field +END +parameters.dat +K 25 +svn:wc:ra_dav:version-url +V 66 +/svn/g_rect_package/!svn/ver/7/trunk/Examples/Field/parameters.dat +END +IMG_6614.JPG +K 25 +svn:wc:ra_dav:version-url +V 64 +/svn/g_rect_package/!svn/ver/2/trunk/Examples/Field/IMG_6614.JPG +END diff --git a/Examples/Field/.svn/entries b/Examples/Field/.svn/entries new file mode 100644 index 0000000000000000000000000000000000000000..2b9a5b91da30bb24333c7801e0f35328c035d463 --- /dev/null +++ b/Examples/Field/.svn/entries @@ -0,0 +1,96 @@ +10 + +dir +7 +http://bourda02@demeter/svn/g_rect_package/trunk/Examples/Field +http://bourda02@demeter/svn/g_rect_package + + + +2013-04-15T22:39:22.457529Z +7 +bourda02 + + + + + + + + + + + + + + +011fb189-60de-4f9a-bef2-e507afd36f24 + +parameters.dat +file + + + + +2013-04-15T22:40:50.000000Z +0977387bbb9e0a144a32eeffb9fdf72a +2013-04-15T22:39:22.457529Z +7 +bourda02 +has-props + + + + + + + + + + + + + + + + + + + + +1563 + +IMG_6614.JPG +file + + + + +2012-03-12T12:14:57.000000Z +4bbd1613b7408c1152f4a65b5f16e96c +2012-12-20T16:51:06.675321Z +2 +bourda02 +has-props + + + + + + + + + + + + + + + + + + + + +970958 + diff --git a/Examples/Field/.svn/prop-base/IMG_6614.JPG.svn-base b/Examples/Field/.svn/prop-base/IMG_6614.JPG.svn-base new file mode 100644 index 0000000000000000000000000000000000000000..5e9587e658c3c3c18ab62ebc908568efd1226aed --- /dev/null +++ b/Examples/Field/.svn/prop-base/IMG_6614.JPG.svn-base @@ -0,0 +1,5 @@ +K 13 +svn:mime-type +V 24 +application/octet-stream +END diff --git a/Examples/Field/.svn/prop-base/parameters.dat.svn-base b/Examples/Field/.svn/prop-base/parameters.dat.svn-base new file mode 100644 index 0000000000000000000000000000000000000000..869ac71cf7e4d72d9ab52f86d630c1c3f0c017ce --- /dev/null +++ b/Examples/Field/.svn/prop-base/parameters.dat.svn-base @@ -0,0 +1,5 @@ +K 14 +svn:executable +V 1 +* +END diff --git a/Examples/Field/.svn/text-base/IMG_6614.JPG.svn-base b/Examples/Field/.svn/text-base/IMG_6614.JPG.svn-base new file mode 100644 index 0000000000000000000000000000000000000000..5f16ab793469466964fdbe86f8174bfc76e0c422 Binary files /dev/null and b/Examples/Field/.svn/text-base/IMG_6614.JPG.svn-base differ diff --git a/Examples/Field/.svn/text-base/parameters.dat.svn-base b/Examples/Field/.svn/text-base/parameters.dat.svn-base new file mode 100644 index 0000000000000000000000000000000000000000..b6621d18555015a7df071ec98d3e107e76835c6e --- /dev/null +++ b/Examples/Field/.svn/text-base/parameters.dat.svn-base @@ -0,0 +1,52 @@ +% I/O information +imgFname = 'IMG_6614.JPG'; +firstImgFname = 'IMG_6614.JPG'; +lastImgFname = 'IMG_6614.JPG'; +outputFname = 'g_rect.mat'; + +% Field or lab case situation. +% Set field = true for field situation and field = false for lab situation. +field = true; + +% Camera position +% lat/lon for field situation +% meter for lab situation +LON0 = -70.6010167; +LAT0 = 47.2713000; + +% Offset from center of the principal point (generally zero) +ic = 0; +jc = 0; + +% Parameters +hfov = 65.0; % Field of view of the camera +lambda = 2; % Dip angle above vertical (e.g. straight down = 90, horizontal = 0) +phi = 0.0; % Tilt angle (generally close to 0). +H = 720; % Camera altitude +theta = 70.0; % View angle clockwise from North (e.g. straight East = 90) + +% Uncertainty in parameters. Set the uncertainty to 0.0 for fixed parameters. +dhfov = 20.0; +dlambda = 10.0; +dphi = 5.0; +dH = 0.0; +dtheta = 20.0; + +% Order of the polynomial correction (0, 1 or 2) +polyOrder = 1; + + +% To save memory calculation can be done in single precision. +% For higher precision set the variable 'precision' to 'double'; +precision = 'double'; + + +% Ground Control Points (GCP). +% The data must come right after the gcpData = true +gcpData = true; + 360 829 -70.561367 47.303783 + 54 719 -70.54500 47.335 + 99 661 -70.505 47.375 + 452 641 -70.435 47.389 + 429 633 -70.418 47.408 + 816 644 -70.393 47.368 \ No newline at end of file diff --git a/Examples/Field/IMG_6614.JPG b/Examples/Field/IMG_6614.JPG new file mode 100644 index 0000000000000000000000000000000000000000..5f16ab793469466964fdbe86f8174bfc76e0c422 Binary files /dev/null and b/Examples/Field/IMG_6614.JPG differ diff --git a/Examples/Field/parameters.dat b/Examples/Field/parameters.dat new file mode 100755 index 0000000000000000000000000000000000000000..b6621d18555015a7df071ec98d3e107e76835c6e --- /dev/null +++ b/Examples/Field/parameters.dat @@ -0,0 +1,52 @@ +% I/O information +imgFname = 'IMG_6614.JPG'; +firstImgFname = 'IMG_6614.JPG'; +lastImgFname = 'IMG_6614.JPG'; +outputFname = 'g_rect.mat'; + +% Field or lab case situation. +% Set field = true for field situation and field = false for lab situation. +field = true; + +% Camera position +% lat/lon for field situation +% meter for lab situation +LON0 = -70.6010167; +LAT0 = 47.2713000; + +% Offset from center of the principal point (generally zero) +ic = 0; +jc = 0; + +% Parameters +hfov = 65.0; % Field of view of the camera +lambda = 2; % Dip angle above vertical (e.g. straight down = 90, horizontal = 0) +phi = 0.0; % Tilt angle (generally close to 0). +H = 720; % Camera altitude +theta = 70.0; % View angle clockwise from North (e.g. straight East = 90) + +% Uncertainty in parameters. Set the uncertainty to 0.0 for fixed parameters. +dhfov = 20.0; +dlambda = 10.0; +dphi = 5.0; +dH = 0.0; +dtheta = 20.0; + +% Order of the polynomial correction (0, 1 or 2) +polyOrder = 1; + + +% To save memory calculation can be done in single precision. +% For higher precision set the variable 'precision' to 'double'; +precision = 'double'; + + +% Ground Control Points (GCP). +% The data must come right after the gcpData = true +gcpData = true; + 360 829 -70.561367 47.303783 + 54 719 -70.54500 47.335 + 99 661 -70.505 47.375 + 452 641 -70.435 47.389 + 429 633 -70.418 47.408 + 816 644 -70.393 47.368 \ No newline at end of file diff --git a/Examples/Lab/.svn/all-wcprops b/Examples/Lab/.svn/all-wcprops new file mode 100644 index 0000000000000000000000000000000000000000..f1c1cc66ced991799232a0f896a610db457df74f --- /dev/null +++ b/Examples/Lab/.svn/all-wcprops @@ -0,0 +1,17 @@ +K 25 +svn:wc:ra_dav:version-url +V 49 +/svn/g_rect_package/!svn/ver/2/trunk/Examples/Lab +END +IMG_8368.jpg +K 25 +svn:wc:ra_dav:version-url +V 62 +/svn/g_rect_package/!svn/ver/2/trunk/Examples/Lab/IMG_8368.jpg +END +parameters.dat +K 25 +svn:wc:ra_dav:version-url +V 64 +/svn/g_rect_package/!svn/ver/2/trunk/Examples/Lab/parameters.dat +END diff --git a/Examples/Lab/.svn/entries b/Examples/Lab/.svn/entries new file mode 100644 index 0000000000000000000000000000000000000000..3f4ac4614e4bc3d9980e616539868adcc88a38a8 --- /dev/null +++ b/Examples/Lab/.svn/entries @@ -0,0 +1,96 @@ +10 + +dir +7 +http://bourda02@demeter/svn/g_rect_package/trunk/Examples/Lab +http://bourda02@demeter/svn/g_rect_package + + + +2012-12-20T16:51:06.675321Z +2 +bourda02 + + + + + + + + + + + + + + +011fb189-60de-4f9a-bef2-e507afd36f24 + +IMG_8368.jpg +file + + + + +2012-12-18T18:09:59.000000Z +2eb6933a87131a15b4eefddf78667242 +2012-12-20T16:51:06.675321Z +2 +bourda02 +has-props + + + + + + + + + + + + + + + + + + + + +2861752 + +parameters.dat +file + + + + +2012-12-18T18:09:59.000000Z +f5f5418e8bfd604a426f5cebd9821688 +2012-12-20T16:51:06.675321Z +2 +bourda02 +has-props + + + + + + + + + + + + + + + + + + + + +1635 + diff --git a/Examples/Lab/.svn/prop-base/IMG_8368.jpg.svn-base b/Examples/Lab/.svn/prop-base/IMG_8368.jpg.svn-base new file mode 100644 index 0000000000000000000000000000000000000000..5e9587e658c3c3c18ab62ebc908568efd1226aed --- /dev/null +++ b/Examples/Lab/.svn/prop-base/IMG_8368.jpg.svn-base @@ -0,0 +1,5 @@ +K 13 +svn:mime-type +V 24 +application/octet-stream +END diff --git a/Examples/Lab/.svn/prop-base/parameters.dat.svn-base b/Examples/Lab/.svn/prop-base/parameters.dat.svn-base new file mode 100644 index 0000000000000000000000000000000000000000..869ac71cf7e4d72d9ab52f86d630c1c3f0c017ce --- /dev/null +++ b/Examples/Lab/.svn/prop-base/parameters.dat.svn-base @@ -0,0 +1,5 @@ +K 14 +svn:executable +V 1 +* +END diff --git a/Examples/Lab/.svn/text-base/IMG_8368.jpg.svn-base b/Examples/Lab/.svn/text-base/IMG_8368.jpg.svn-base new file mode 100644 index 0000000000000000000000000000000000000000..4c8b65a63ee23af9d4d34990d2acea6bb93d5153 Binary files /dev/null and b/Examples/Lab/.svn/text-base/IMG_8368.jpg.svn-base differ diff --git a/Examples/Lab/.svn/text-base/parameters.dat.svn-base b/Examples/Lab/.svn/text-base/parameters.dat.svn-base new file mode 100644 index 0000000000000000000000000000000000000000..79ead0d4043593d2cb0ba3294e6a31d9ec3381ed --- /dev/null +++ b/Examples/Lab/.svn/text-base/parameters.dat.svn-base @@ -0,0 +1,67 @@ + + +% I/O information +imgFname = 'IMG_8368.jpg'; +firstImgFname = 'IMG_8368.jpg'; +lastImgFname = 'IMG_8368.jpg'; +outputFname = 'IMG_8368.mat'; + +% Field or lab case situation. +% Set field = true for field situation and field = false for lab situation. +field = false; + +% Camera position +% lat/lon for field situation +% meter for lab situation +LON0 = 1.01; +LAT0 = 2.36; + +% Offset from center of the principal point (generally zero) +ic = 0; +jc = 0; + +% Parameters +hfov = 62.00; % Field of view of the camera +lambda = 53.0; % Dip angle below horizontal (e.g. straight down = 90, horizontal = 0) +phi = 1.0; % Tilt angle (generally close to 0). +H = 1.755; % Camera altitude (m) +theta = 180.0; % View angle anticlockwise from North (e.g. straight East = 270) + +% Uncertainty in parameters. Set the uncertainty to 0.0 for fixed parameters. +dhfov = 5.0; +dlambda = 10.0; +dphi = 5.0; +dH = 0.5; +dtheta = 20.0; + +% Order of the polynomial correction (0, 1 or 2) +polyOrder = 2; + + +% To save memory calculation can be done in single precision. +% For higher precision set the variable 'precision' to 'double'; +precision = 'double'; + + +% Ground Control Points (GCP). +gcpData = true; +2999 226 0.500 0.00 +1694 220 1.500 0.00 + 528 677 2.290 0.50 + 289 1231 2.290 1.00 +3819 686 0.000 0.50 +4018 1266 0.000 1.00 +2566 1235 0.890 0.99 +3806 2118 0.255 1.56 +4131 1580 0.000 1.23 + 521 2548 1.910 1.82 + 248 2328 2.060 1.71 + 14 1868 2.290 1.46 + + + + + + + + diff --git a/Examples/Lab/IMG_8368.jpg b/Examples/Lab/IMG_8368.jpg new file mode 100644 index 0000000000000000000000000000000000000000..4c8b65a63ee23af9d4d34990d2acea6bb93d5153 Binary files /dev/null and b/Examples/Lab/IMG_8368.jpg differ diff --git a/Examples/Lab/parameters.dat b/Examples/Lab/parameters.dat new file mode 100755 index 0000000000000000000000000000000000000000..79ead0d4043593d2cb0ba3294e6a31d9ec3381ed --- /dev/null +++ b/Examples/Lab/parameters.dat @@ -0,0 +1,67 @@ + + +% I/O information +imgFname = 'IMG_8368.jpg'; +firstImgFname = 'IMG_8368.jpg'; +lastImgFname = 'IMG_8368.jpg'; +outputFname = 'IMG_8368.mat'; + +% Field or lab case situation. +% Set field = true for field situation and field = false for lab situation. +field = false; + +% Camera position +% lat/lon for field situation +% meter for lab situation +LON0 = 1.01; +LAT0 = 2.36; + +% Offset from center of the principal point (generally zero) +ic = 0; +jc = 0; + +% Parameters +hfov = 62.00; % Field of view of the camera +lambda = 53.0; % Dip angle below horizontal (e.g. straight down = 90, horizontal = 0) +phi = 1.0; % Tilt angle (generally close to 0). +H = 1.755; % Camera altitude (m) +theta = 180.0; % View angle anticlockwise from North (e.g. straight East = 270) + +% Uncertainty in parameters. Set the uncertainty to 0.0 for fixed parameters. +dhfov = 5.0; +dlambda = 10.0; +dphi = 5.0; +dH = 0.5; +dtheta = 20.0; + +% Order of the polynomial correction (0, 1 or 2) +polyOrder = 2; + + +% To save memory calculation can be done in single precision. +% For higher precision set the variable 'precision' to 'double'; +precision = 'double'; + + +% Ground Control Points (GCP). +gcpData = true; +2999 226 0.500 0.00 +1694 220 1.500 0.00 + 528 677 2.290 0.50 + 289 1231 2.290 1.00 +3819 686 0.000 0.50 +4018 1266 0.000 1.00 +2566 1235 0.890 0.99 +3806 2118 0.255 1.56 +4131 1580 0.000 1.23 + 521 2548 1.910 1.82 + 248 2328 2.060 1.71 + 14 1868 2.290 1.46 + + + + + + + + diff --git a/src/g_calib/Compute3D.m b/src/g_calib/Compute3D.m new file mode 100755 index 0000000000000000000000000000000000000000..7d1872a62252fec71b4822795762618f1b281655 --- /dev/null +++ b/src/g_calib/Compute3D.m @@ -0,0 +1,90 @@ +function [Xc,Xp] = Compute3D(xc,xp,R,T,fc,fp,cc,cp,kc,kp,alpha_c,alpha_p); + +% [Xc,Xp] = Compute3D(xc,xp,R,T,fc,fp,cc,cp,kc,kp,alpha_c,alpha_p); +% +% Reconstruction of the 3D structure of the striped object. +% +% Xc : The 3D coordinates of the points in the camera reference frame +% Xp : The 3D coordinates of the points in the projector reference frame +% +% xc, xp: Camera coordinates and projector coordinates from ComputeStripes +% R,T : rigid motion from camera to projector: Xp = R*Xc + T +% fc,fp : Camera and Projector focal lengths +% cc,cp : Camera and Projector center of projection +% kc,kp : Camera and Projector distortion factors +% alpha_c, alpha_p: skew coefficients for camera and projector +% +% The set R,T,fc,fp,cc,cp and kc comes from the calibration. + +% Intel Corporation - Dec. 2003 +% (c) Jean-Yves Bouguet + + +if nargin < 12, + alpha_p = 0; + if nargin < 11, + alpha_c = 0; + end; +end; + + +Np = size(xc,2); + + +xc = normalize_pixel(xc,fc,cc,kc,alpha_c); + +xp = (xp - cp(1))/fp(1); + +xp_save = xp; % save the real distorted x - coordinates + alpha'ed + + +if (norm(kp) == 0)&(alpha_p == 0), + N_rep = 1; +else + N_rep = 5; +end; + + +% xp is the first entry of the undistorted projector coordinates (iteratively refined) +% xc is the complete undistorted camera coordinates +for kk = 1:N_rep, + + R2 = R([1 3],:); + if length(T) > 2, + Tp = T([1 3]); % The old technique for calibration + else + Tp = T; % The new technique for calibration (using stripes only) + end; + + % Triangulation: + + D1 = [-xc(1,:);xc(1,:).*xp(1,:);-xc(2,:);xc(2,:).*xp(1,:);-ones(1,Np);xp(1,:)]; + D2 = R2(:)*ones(1,Np); + + D = sum(D1.*D2); + N1 = [-ones(1,Np);xp(1,:)]; + N2 = -sum(N1.*(Tp*ones(1,Np))); + Z = N2./D; + Xc = (ones(3,1)*Z).*[xc;ones(1,Np)]; + + % reproject on the projetor view, and apply distortion... + + Xp = R*Xc + T*ones(1,Np); + + xp_v = [Xp(1,:)./Xp(3,:); Xp(2,:)./Xp(3,:)]; + + xp_v(1,:) = xp_v(1,:) + alpha_p * xp_v(2,:); + + xp_dist = apply_distortion(xp_v,kp); + + %norm(xp_dist(1,:) - xp_save) + + xp_dist(1,:) = xp_save; + + xp_v = comp_distortion(xp_dist,kp); + + xp_v(1,:) = xp_v(1,:) - alpha_p * xp_v(2,:); + + xp = xp_v(1,:); + +end; diff --git a/src/g_calib/ComputeStripes.m b/src/g_calib/ComputeStripes.m new file mode 100755 index 0000000000000000000000000000000000000000..b90108fad3abd31ebea68b97024e470c741ee966 --- /dev/null +++ b/src/g_calib/ComputeStripes.m @@ -0,0 +1,378 @@ +function [xc,xp,nx,ny] = ComputeStripes(fname,graphout); + +%[xc,xp] = ComputeStripes(fname,graphout) +% +% This function computes the projector stripe coordinate (at subpixel +% accuracy) at every pixel in the image. The algorithm used in temporal +% processing (with a translationnal pattern of period 32 pixels). A coarse +% to fine pattern projection helpresolve for the period ambiguity (in a +% Gray-Code Scheme). +% +% The naming convention is, +% +% fname_pat##p.ras --> for the positive image from pass ## +% fname_pat##n.ras --> for the negative image from pass ## +% +% ##=00: Black and White images +% +% ##=[01 - 10] : Coarse to fine projection (Gary-Scale projection) +% ##=[11 - 42 ]: 32 translational patterns of period 32 pixels (for +% temporal processing) +% INPUT: +% fname --> base name of the images +% graphout --> Set to 1 to show graphical figures +% +% OUTPUT: +% xc is a 2 by N matrix of the point in the image plane +% (convention: (0,0) -> center of the upper left pixel in the camera image) +% xp is a N vector of the corresponding projector stripe numbers (at subpixel accuracy). +% (convention: (0,0) -> center of the upper left pixel in the projector image) +% (nx,ny): Size of the camera image +% +% (c) in 1996 by Jean-Yves Bouguet - Updated 11/26/2003 + + +if nargin < 2, + graphout = 0; +end; + + +N = 10; + + +%-- Load the white and black images: + +blackIm = imread([fname '_pat00p.bmp']); +whiteIm = imread([fname '_pat00n.bmp']); + +if size(blackIm,3) > 1, + blackIm = 0.299 * double(blackIm(:,:,1)) + 0.5870 * double(blackIm(:,:,2)) + 0.114 * double(blackIm(:,:,3)); + whiteIm = 0.299 * double(whiteIm(:,:,1)) + 0.5870 * double(whiteIm(:,:,2)) + 0.114 * double(whiteIm(:,:,3)); +else + blackIm = double(blackIm); + whiteIm = double(whiteIm); +end; + +[ny,nx] = size(blackIm); + + +%%% Contrast Thresholding + +%% good value for cthresh: 20 + +totContr = whiteIm - blackIm; +totContr(totContr < 0) = 0; + +cthresh = 3; %--> totContr is larger than cthresh for valid pixels + +% In order to remove the highlights (reject image regions where whiteIm >254) +hightlight_reject = 1; + + + +%-- Enable processing of a small region of the image instead of the whole image: + +xs = 1; +xe = nx; +ys = 1; +ye = ny; + +totContr = totContr(ys:ye,xs:xe); +whiteIm = whiteIm(ys:ye,xs:xe); +blackIm = blackIm(ys:ye,xs:xe); + +[yPixels xPixels] = size(totContr); + + +good1 = find((totContr > cthresh)&(whiteIm <= 254)); % the first mask!!! (no need to compute anything outside of this mask) +Ng1 = length(good1); + + + + + +%-------------------------------------------------------------------------- +%-- STEP 1: TEMPORAL PROCESSING -> Finding the edge time at every pixel in the image +%-------------------------------------------------------------------------- + +period = 32; % Total Period size in pixels of the projected pattern + +index_list2 = (0:period-1) + N + 1; + + +% Read all temporal images (and compute max and min images): + +for kk=0:period-1, + + tmp = imread([fname '_pat' sprintf('%.2d',index_list2(kk+1)) 'p.bmp']); + + if size(tmp,3) > 1, + tmp = 0.299 * double(tmp(ys:ye,xs:xe,1)) + 0.5870 * double(tmp(ys:ye,xs:xe,2)) + 0.114 * double(tmp(ys:ye,xs:xe,3)); + else + tmp = double(tmp(ys:ye,xs:xe)); + end; + + + eval(['I_' num2str(kk) '= tmp;']); + + if kk == 0, + Imin = tmp; + Imax = tmp; + else + Imin = min(Imin,tmp); + Imax = max(Imax,tmp); + end; + +end; + + +% Substract opposite images (to compute a zero crossing): +for kk = 0:period-1, + + eval(['I_kk = I_' num2str(kk) ';']); + eval(['I_kk2 = I_' num2str(mod(kk+period/2,period)) ';']); + + eval(['J_' num2str(kk) ' = I_kk - I_kk2;']); + +end; + + +% Start computing the edge points: + +not_computed = ones(yPixels,xPixels); +xp_crossings = -ones(yPixels,xPixels); + + +for kk = 1:period, + + eval(['Ja = J_' num2str(mod(kk-3,period)) ';']); + eval(['Jb = J_' num2str(mod(kk-2,period)) ';']); + eval(['Jc = J_' num2str(mod(kk-1,period)) ';']); + eval(['Jd = J_' num2str(mod(kk,period)) ';']); + eval(['Je = J_' num2str(mod(kk+1,period)) ';']); + eval(['Jf = J_' num2str(mod(kk+2,period)) ';']); + + % Temporal Smoothing: (before zero crossing computation) + J_current = (Jb + 4*Jc + 6*Jd + 4*Je + Jf)/16; + J_prev = (Ja + 4*Jb + 6*Jc + 4*Jd + Je)/16; + + ind_found = find( (J_current >= 0) & (J_prev < 0) & (not_computed) ); + + J_current = J_current(ind_found); + J_prev = J_prev(ind_found); + + xp_crossings(ind_found) = (kk - (J_current ./ (J_current - J_prev))) - 0.5; + + not_computed(ind_found) = zeros(length(ind_found),1); + +end; + +% Final temporal solution: +xp_crossings = mod(xp_crossings,period); + +if graphout, + figure(3); + image(xp_crossings*8); + colormap(gray(256)); + title('STEP1: Subpixel projector coordinate with a 32 pixel ambiguity'); + drawnow; +end; + + +%-------------------------------------------------------------------------- +%-- STEP 2: SPATIAL PROCESSING -> Coarse to fine processing to resolve ambiguity +%-------------------------------------------------------------------------- + +bin_current = zeros(yPixels,xPixels); + +num_period = zeros(yPixels,xPixels); + +for i = 1:N-log2(period), + + + tmpN = imread([fname '_pat' sprintf('%.2d',i) 'p.bmp']); + tmpI = imread([fname '_pat' sprintf('%.2d',i) 'n.bmp']); + + if size(tmpN,3) > 1, + tmpN = 0.299 * double(tmpN(ys:ye,xs:xe,1)) + 0.5870 * double(tmpN(ys:ye,xs:xe,2)) + 0.114 * double(tmpN(ys:ye,xs:xe,3)); + tmpI = 0.299 * double(tmpI(ys:ye,xs:xe,1)) + 0.5870 * double(tmpI(ys:ye,xs:xe,2)) + 0.114 * double(tmpI(ys:ye,xs:xe,3)); + else + tmpN = double(tmpN(ys:ye,xs:xe)); + tmpI = double(tmpI(ys:ye,xs:xe)); + end; + + diffI = (tmpN-tmpI)>0; + + bin_current = xor(bin_current,diffI); + + num_period = num_period + (2^(N-i))*bin_current; + +end; + + +if graphout, + figure(4); + image(num_period/4); + colormap(gray(256)); + title('STEP2: Period number (for removing the periodic ambiguity)'); + drawnow; +end; + + +% Finish off the spatial processing to higher resolution: + +xp_spatial = num_period; + +finer_image = 2; + +for i = N-log2(period)+1:N-finer_image+1, + + + tmpN = imread([fname '_pat' sprintf('%.2d',i) 'p.bmp']); + tmpI = imread([fname '_pat' sprintf('%.2d',i) 'n.bmp']); + + if size(tmpN,3) > 1, + tmpN = 0.299 * double(tmpN(ys:ye,xs:xe,1)) + 0.5870 * double(tmpN(ys:ye,xs:xe,2)) + 0.114 * double(tmpN(ys:ye,xs:xe,3)); + tmpI = 0.299 * double(tmpI(ys:ye,xs:xe,1)) + 0.5870 * double(tmpI(ys:ye,xs:xe,2)) + 0.114 * double(tmpI(ys:ye,xs:xe,3)); + else + tmpN = double(tmpN(ys:ye,xs:xe)); + tmpI = double(tmpI(ys:ye,xs:xe)); + end; + + diffI = (tmpN-tmpI)>0; + + bin_current = xor(bin_current,diffI); + + xp_spatial = xp_spatial + (2^(N-i))*bin_current; + +end; + + +% Final spatial solution: +xp_spatial = xp_spatial + (2^(finer_image-1)-1); + + + + +%-------------------------------------------------------------------------- +%-- STEP 3: Solve for periodic ambiguity, and fixing gliches of the temporal processing (due to noise) +% In order to compare xp_spatial and xp_crossings; Not discussed in class +%-------------------------------------------------------------------------- + + +% Fix glitches at the stripe boundaries (of width 4 pixels): + +for kkk = 1:10, + pos_cand = ((num_period == ([1e10*ones(yPixels,1) num_period(:,1:end-1)]+period)) | (num_period == ([1e10*ones(yPixels,2) num_period(:,1:end-2)]+period)))&(xp_crossings > 5*period /6); + neg_cand = ((num_period == ([num_period(:,2:end) zeros(yPixels,1)]-period)) | (num_period == ([num_period(:,3:end) zeros(yPixels,2)]-period)))&(xp_crossings < period /6); + num_period = num_period - period*pos_cand + period*neg_cand; +end; + +xp_crossings2 = xp_crossings + num_period; + +period3 = period / 2; + +% Fix the little glitch at the stripe boundaries: + +% Find single glitches: + +for kkk = 1:5, + delta_x = xp_crossings2(:,2:end)-xp_crossings2(:,1:end-1); + + pos_glitch = (delta_x > 3*period3/4)&(delta_x < 3*period3); + neg_glitch = (delta_x < -period3/4)&(delta_x > -3*period3); + no_glitch = ~pos_glitch & ~neg_glitch; + + % Place to subtract a period: + sub_places = [ (neg_glitch & [zeros(yPixels,1) pos_glitch(:,1:end-1)]) zeros(yPixels,1)] ; + add_places = [ (pos_glitch & [zeros(yPixels,1) neg_glitch(:,1:end-1)]) zeros(yPixels,1)] ; + + xp_crossings3 = xp_crossings2 - period3 * sub_places + period3 * add_places; + + xp_crossings2 = xp_crossings3; + +end; + +% Find double glitches: + +for kkk = 1:5, + delta_x = xp_crossings2(:,2:end)-xp_crossings2(:,1:end-1); + pos_glitch = (delta_x > 3*period3/4)&(delta_x < 3*period3); + neg_glitch = (delta_x < -period3/4)&(delta_x > -3*period3); + no_glitch = ~pos_glitch & ~neg_glitch; + + sub2_places = [((no_glitch)& [zeros(yPixels,1) pos_glitch(:,1:end-1)] & [neg_glitch(:,2:end) zeros(yPixels,1)])|(neg_glitch & [zeros(yPixels,1) no_glitch(:,1:end-1)] & [zeros(yPixels,2) pos_glitch(:,1:end-2)]) zeros(yPixels,1)]; + add2_places = [((no_glitch)& [zeros(yPixels,1) neg_glitch(:,1:end-1)] & [pos_glitch(:,2:end) zeros(yPixels,1)])|(pos_glitch & [zeros(yPixels,1) no_glitch(:,1:end-1)] & [zeros(yPixels,2) neg_glitch(:,1:end-2)]) zeros(yPixels,1)]; + + xp_crossings3 = xp_crossings2 - period3 * sub2_places + period3 * add2_places; + + xp_crossings2 = xp_crossings3; +end; + + +% End fix + + +if graphout, + figure(5); + image(xp_crossings2/4); + colormap(gray(256)); + title('STEP3: Subpixel projector coordinate without the 32 pixel ambiguity'); + drawnow; +end; + + +%-------------------------------------------------------------------------- +%-- STEP 4: Compare xp_spatial and xp_crossings2 and retains the +% xp_crossings that are valid +%-------------------------------------------------------------------------- + +%comparison of spatial and temporal xp for rejecting the bad pixels: +err_xp = xp_spatial - xp_crossings2; + +spatial_temporal_agree = (err_xp <= 2^(finer_image-1)) & (err_xp > -1); + +mask_temporal = zeros(yPixels,xPixels); +mask_temporal(2:(yPixels-1),2:(xPixels-1)) = ones(yPixels-2,xPixels-2); + +if hightlight_reject, + highlight = (whiteIm > 254); + mask_good = (totContr > cthresh) & (not_computed >= 0) & mask_temporal & spatial_temporal_agree & ~highlight; +else + mask_good = (totContr > cthresh) & (not_computed >= 0) & mask_temporal & spatial_temporal_agree; +end; + +good1 = find(mask_good); + + +xp_crossings3 = xp_crossings2; + +xp_crossings3(~mask_good) = NaN; + + +if graphout, + figure(6); + image(xp_crossings3/4); + colormap(gray(256)); + title('STEP4: Final clean subpixel projector coordinates'); + drawnow; +end; + + +%-------------------------------------------------------------------------- +%-- STEP 5: Produce the camera coordinates and the projector coordinates xc, xp +%-------------------------------------------------------------------------- + +% extract the good pixels only: +xp = xp_crossings2(good1)'; + +[X,Y] = meshgrid(0:xPixels-1,0:yPixels-1); + +xc = [X(good1)';Y(good1)']; + +%%% Express the coordinates of the points in the original +%%% image coordinates: + +xc(1,:) = xc(1,:) + xs - 1; +xc(2,:) = xc(2,:) + ys - 1; + diff --git a/src/g_calib/Distor2Calib.m b/src/g_calib/Distor2Calib.m new file mode 100755 index 0000000000000000000000000000000000000000..a82f58394a59b5b755e554f7fc66a0eec39e807c --- /dev/null +++ b/src/g_calib/Distor2Calib.m @@ -0,0 +1,391 @@ +function [fc_2,Rc_2,Tc_2,H_2,distance,V_vert,V_hori,x_all_c,V_hori_pix,V_vert_pix,V_diag1_pix,V_diag2_pix]=Distor2Calib(k_dist,grid_pts_centered,n_sq_x,n_sq_y,Np,W,L,Xgrid_2,f_ini,N_iter,two_focal); + +% Computes the calibration parameters knowing the +% distortion factor k_dist + +% grid_pts_centered are the grid point coordinates after substraction of +% the optical center. + +% can give an optional guess for the focal length f_ini (can set to []) +% can provide the number of iterations for the Iterative Vanishing Point Algorithm + +% if the focal length is known perfectly, then, there is no need to iterate, +% and therefore, one can fix: N_iter = 0; + +% California Institute of Technology +% (c) Jean-Yves Bouguet - October 7th, 1997 + + + +%keyboard; + +if exist('two_focal'), + if isempty(two_focal), + two_focal=0; + end; +else + two_focal = 0; +end; + + +if exist('N_iter'), + if ~isempty(N_iter), + disp('Use number of iterations provided'); + else + N_iter = 10; + end; +else + N_iter = 10; +end; + +if exist('f_ini'), + if ~isempty(f_ini), + disp('Use focal provided'); + if length(f_ini)<2, f_ini=[f_ini;f_ini]; end; + fc_2 = f_ini; + x_all_c = [grid_pts_centered(1,:)/fc_2(1);grid_pts_centered(2,:)/fc_2(2)]; + x_all_c = comp_distortion(x_all_c,k_dist); % we can this time!!! + else + fc_2 = [1;1]; + x_all_c = grid_pts_centered; + end; +else + fc_2 = [1;1]; + x_all_c = grid_pts_centered; +end; + + +dX = W/n_sq_x; +dY = L/n_sq_y; + + +N_x = n_sq_x+1; +N_y = n_sq_y+1; + + +x_grid = zeros(N_x,N_y); +y_grid = zeros(N_x,N_y); + + + + + +%%% Computation of the four vanishing points in pixels + + + x_grid(:) = grid_pts_centered(1,:); + y_grid(:) = grid_pts_centered(2,:); + + for k=1:n_sq_x+1, + [U,S,V] = svd([x_grid(k,:);y_grid(k,:);ones(1,n_sq_y+1)]); + vert(:,k) = U(:,3); + end; + + for k=1:n_sq_y+1, + [U,S,V] = svd([x_grid(:,k)';y_grid(:,k)';ones(1,n_sq_x+1)]); + hori(:,k) = U(:,3); + end; + + % 2 principle Vanishing points: + [U,S,V] = svd(vert); + V_vert = U(:,3); + [U,S,V] = svd(hori); + V_hori = U(:,3); + + + + % Square warping: + + + vert_first = vert(:,1) - dot(V_vert,vert(:,1))/dot(V_vert,V_vert) * V_vert; + vert_last = vert(:,n_sq_x+1) - dot(V_vert,vert(:,n_sq_x+1))/dot(V_vert,V_vert) * V_vert; + + hori_first = hori(:,1) - dot(V_hori,hori(:,1))/dot(V_hori,V_hori) * V_hori; + hori_last = hori(:,n_sq_y+1) - dot(V_hori,hori(:,n_sq_y+1))/dot(V_hori,V_hori) * V_hori; + + + x1 = cross(hori_first,vert_first); + x2 = cross(hori_first,vert_last); + x3 = cross(hori_last,vert_last); + x4 = cross(hori_last,vert_first); + + x1 = x1/x1(3); + x2 = x2/x2(3); + x3 = x3/x3(3); + x4 = x4/x4(3); + + + + [square] = Rectangle2Square([x1 x2 x3 x4],W,L); + + y1 = square(:,1); + y2 = square(:,2); + y3 = square(:,3); + y4 = square(:,4); + + H2 = cross(V_vert,V_hori); + + V_diag1 = cross(cross(y1,y3),H2); + V_diag2 = cross(cross(y2,y4),H2); + + V_diag1 = V_diag1 / norm(V_diag1); + V_diag2 = V_diag2 / norm(V_diag2); + + V_hori_pix = V_hori; + V_vert_pix = V_vert; + V_diag1_pix = V_diag1; + V_diag2_pix = V_diag2; + + +% end of computation of the vanishing points in pixels. + + + + + + + + +if two_focal, % only if we attempt to estimate two focals... + % Use diagonal lines also to add two extra vanishing points (?) + N_min = min(N_x,N_y); + + if N_min < 2, + use_diag = 0; + two_focal = 0; + disp('Cannot estimate two focals (no existing diagonals)'); + else + use_diag = 1; + Delta_N = abs(N_x-N_y); + N_extra = round((N_min - Delta_N - 1)/2); + diag_list = -N_extra:Delta_N+N_extra; + N_diag = length(diag_list); + diag_1 = zeros(3,N_diag); + diag_2 = zeros(3,N_diag); + end; +else + % Give up the use of the diagonals (so far) + % it seems that the error is increased + use_diag = 0; +end; + + + +% The vertical lines: vert, Horizontal lines: hori +vert = zeros(3,n_sq_x+1); +hori = zeros(3,n_sq_y+1); + +for counter_k = 1:N_iter, % the Iterative Vanishing Points Algorithm to + % estimate the focal length accurately + + x_grid(:) = x_all_c(1,:); + y_grid(:) = x_all_c(2,:); + + for k=1:n_sq_x+1, + [U,S,V] = svd([x_grid(k,:);y_grid(k,:);ones(1,n_sq_y+1)]); + vert(:,k) = U(:,3); + end; + + for k=1:n_sq_y+1, + [U,S,V] = svd([x_grid(:,k)';y_grid(:,k)';ones(1,n_sq_x+1)]); + hori(:,k) = U(:,3); + end; + + % 2 principle Vanishing points: + [U,S,V] = svd(vert); + V_vert = U(:,3); + [U,S,V] = svd(hori); + V_hori = U(:,3); + + + + % Square warping: + + + vert_first = vert(:,1) - dot(V_vert,vert(:,1))/dot(V_vert,V_vert) * V_vert; + vert_last = vert(:,n_sq_x+1) - dot(V_vert,vert(:,n_sq_x+1))/dot(V_vert,V_vert) * V_vert; + + hori_first = hori(:,1) - dot(V_hori,hori(:,1))/dot(V_hori,V_hori) * V_hori; + hori_last = hori(:,n_sq_y+1) - dot(V_hori,hori(:,n_sq_y+1))/dot(V_hori,V_hori) * V_hori; + + + x1 = cross(hori_first,vert_first); + x2 = cross(hori_first,vert_last); + x3 = cross(hori_last,vert_last); + x4 = cross(hori_last,vert_first); + + x1 = x1/x1(3); + x2 = x2/x2(3); + x3 = x3/x3(3); + x4 = x4/x4(3); + + + + [square] = Rectangle2Square([x1 x2 x3 x4],W,L); + + y1 = square(:,1); + y2 = square(:,2); + y3 = square(:,3); + y4 = square(:,4); + + H2 = cross(V_vert,V_hori); + + V_diag1 = cross(cross(y1,y3),H2); + V_diag2 = cross(cross(y2,y4),H2); + + V_diag1 = V_diag1 / norm(V_diag1); + V_diag2 = V_diag2 / norm(V_diag2); + + + + + % Estimation of the focal length, and normalization: + + % Compute the ellipsis of (1/f^2) positions: + % a * (1/fx)^2 + b * (1/fx)^2 = -c + + + a1 = V_hori(1); + b1 = V_hori(2); + c1 = V_hori(3); + + a2 = V_vert(1); + b2 = V_vert(2); + c2 = V_vert(3); + + a3 = V_diag1(1); + b3 = V_diag1(2); + c3 = V_diag1(3); + + a4 = V_diag2(1); + b4 = V_diag2(2); + c4 = V_diag2(3); + + + if two_focal, + + + A = [a1*a2 b1*b2;a3*a4 b3*b4]; + b = -[c1*c2;c3*c4]; + + f = sqrt(abs(1./(inv(A)*b))); + + else + + f = sqrt(abs(-(c1*c2*(a1*a2 + b1*b2) + c3*c4*(a3*a4 + b3*b4))/(c1^2*c2^2 + c3^2*c4^2))); + + f = [f;f]; + + end; + + + + % REMARK: + % if both a and b are small, the calibration is impossible. + % if one of them is small, only the other focal length is observable + % if none is small, both focals are observable + + + fc_2 = fc_2 .* f; + + + % DEBUG PART: fix focal to 500... + %fc_2= [500;500]; disp('Line 293 to be earased in Distor2Calib.m'); + + + % end of focal compensation + + % normalize by the current focal: + + x_all = [grid_pts_centered(1,:)/fc_2(1);grid_pts_centered(2,:)/fc_2(2)]; + + % Compensate by the distortion factor: + + x_all_c = comp_distortion(x_all,k_dist); + +end; + +% At that point, we hope that the distortion is gone... + +x_grid(:) = x_all_c(1,:); +y_grid(:) = x_all_c(2,:); + +for k=1:n_sq_x+1, + [U,S,V] = svd([x_grid(k,:);y_grid(k,:);ones(1,n_sq_y+1)]); + vert(:,k) = U(:,3); +end; + +for k=1:n_sq_y+1, + [U,S,V] = svd([x_grid(:,k)';y_grid(:,k)';ones(1,n_sq_x+1)]); + hori(:,k) = U(:,3); +end; + +% Vanishing points: +[U,S,V] = svd(vert); +V_vert = U(:,3); +[U,S,V] = svd(hori); +V_hori = U(:,3); + +% Horizon: + +H_2 = cross(V_vert,V_hori); + +% H_2 = cross(V_vert,V_hori); + +% pick a plane in front of the camera (positive depth) +if H_2(3) < 0, H_2 = -H_2; end; + + +% Rotation matrix: + +if V_hori(1) < 0, V_hori = -V_hori; end; + +V_hori = V_hori/norm(V_hori); +H_2 = H_2/norm(H_2); + +V_hori = V_hori - dot(V_hori,H_2)*H_2; + +Rc_2 = [V_hori cross(H_2,V_hori) H_2]; + +Rc_2 = Rc_2 / det(Rc_2); + +%omc_2 = rodrigues(Rc_2); + +%Rc_2 = rodrigues(omc_2); + +% Find the distance of the plane for translation vector: + +xc_2 = [x_all_c;ones(1,Np)]; + +Zc_2 = 1./sum(xc_2 .* (Rc_2(:,3)*ones(1,Np))); + +Xo_2 = [sum(xc_2 .* (Rc_2(:,1)*ones(1,Np))).*Zc_2 ; sum(xc_2 .* (Rc_2(:,2)*ones(1,Np))).*Zc_2]; + +XXo_2 = Xo_2 - mean(Xo_2')'*ones(1,Np); + +distance_x = norm(Xgrid_2(1,:))/norm(XXo_2(1,:)); +distance_y = norm(Xgrid_2(2,:))/norm(XXo_2(2,:)); + + +distance = sum(sum(XXo_2(1:2,:).*Xgrid_2(1:2,:)))/sum(sum(XXo_2(1:2,:).^2)); + +alpha = abs(distance_x - distance_y)/distance; + +if (alpha>0.1)&~two_focal, + disp('Should use two focals in x and y...'); +end; + +% Deduce the translation vector: + +Tc_2 = distance * H_2; + + + + + +return; + + V_hori_pix/V_hori_pix(3) + V_vert_pix/V_vert_pix(3) + V_diag1_pix/V_diag1_pix(3) + V_diag2_pix/V_diag2_pix(3) diff --git a/src/g_calib/Meshing.m b/src/g_calib/Meshing.m new file mode 100755 index 0000000000000000000000000000000000000000..59a35079410c8e1dbf330c63411ae0e6fea36b69 --- /dev/null +++ b/src/g_calib/Meshing.m @@ -0,0 +1,458 @@ +function [Xc3,tri3,xc3,xp3,dc3,xc_texture,nc3,conf_nc3,Nn3] = Meshing(Xc2,xc2,xp2,Thresh_connect,N_smoothing,om,T,N_x,N_y,fc,cc,kc,alpha_c,fp,cp,kp,alpha_p), + +% scaled connection threshold + +T_connect = Thresh_connect; % scaled threshold + +fprintf(1,'Organizing the data...\n'); + +xp_frac = rem(xp2,1); +xc_frac = rem(xc2(1,:),1); + +if std(xp_frac) > std(xc_frac), + disp('Dense depth map in the image coordinates'); + temporal = 1; + spatial = 0; +else + disp('Dense depth map in the cross image and projector frame'); + temporal = 0; + spatial = 1; +end; + + +if spatial, + + % Something to fix the organization: + xpmin = min(xp2); + xp_ind = round(unique(xp2 - xpmin)); + step_xp = min(diff(xp_ind)); %xp_ind(2) - xp_ind(1); + xp4 = (xp2 - xpmin)/step_xp + xpmin; + + xpmin = min(xp4); + xpmax = max(xp4); + + xcmin = min(xc2(2,:)); + xcmax = max(xc2(2,:)); + +else + + % Something to fix the organization: + + xp4 = xc2(1,:); + + xpmin = min(xp4); + xpmax = max(xp4); + + xcmin = min(xc2(2,:)); + xcmax = max(xc2(2,:)); + +end; + + +Nrow = xcmax - xcmin + 1; +Ncol = xpmax - xpmin + 1; + +Xmesh = zeros(Nrow,Ncol); +Ymesh = zeros(Nrow,Ncol); +Zmesh = zeros(Nrow,Ncol); +Cmesh = zeros(Nrow,Ncol); +xcmesh = zeros(Nrow,Ncol); +ycmesh = zeros(Nrow,Ncol); + +ind_col = round(xp4 - xpmin + 1); +ind_row = xc2(2,:) - xcmin + 1; +ind = ind_row + (ind_col-1)*Nrow; +ni = length(ind); + + +% Good format for ivview: +Xmesh(ind) = Xc2(1,:); +Ymesh(ind) = Xc2(2,:); % tries to have it in the right position +Zmesh(ind) = Xc2(3,:); % tries to have it in the right position +Cmesh(ind) = ones(1,ni); +xcmesh(ind) = xc2(1,:); +ycmesh(ind) = xc2(2,:); + +% Hypothesis on the triangles: + +D1 = Cmesh(2:Nrow,1:(Ncol-1)) & Cmesh(1:(Nrow-1),2:Ncol); +F1 = Cmesh(1:(Nrow-1),1:(Ncol-1)) & D1; +F2 = Cmesh(2:Nrow,2:Ncol) & D1; +C1 = (F1 | F2); + +D2 = Cmesh(1:(Nrow-1),1:(Ncol-1)) & Cmesh(2:Nrow,2:Ncol); +F3 = Cmesh(1:(Nrow-1),2:Ncol) & D2; +F4 = Cmesh(2:Nrow,1:(Ncol-1)) & D2; +C2 = (F3 | F4); + +Ambi = C1 & C2; % ambiguous zones +Div1 = C1 & ~C2; % needs to check relative distance of points +Div2 = ~C1 & C2; % needs to check relative distance of points + +% diagonal measure: + +%Dm1 = abs(Zmesh(2:Nrow,1:(Ncol-1)) - Zmesh(1:(Nrow-1),2:Ncol)); + + +Dm1 =( Xmesh(2:Nrow,1:(Ncol-1)) - Xmesh(1:(Nrow-1),2:Ncol)).^2 + (Ymesh(2:Nrow,1:(Ncol-1)) - Ymesh(1:(Nrow-1),2:Ncol)).^2 + (Zmesh(2:Nrow,1:(Ncol-1)) - Zmesh(1:(Nrow-1),2:Ncol)).^2; + +%Dm2 = abs(Zmesh(1:(Nrow-1),1:(Ncol-1)) - Zmesh(2:Nrow,2:Ncol)); + +Dm2 = (Xmesh(1:(Nrow-1),1:(Ncol-1)) - Xmesh(2:Nrow,2:Ncol)).^2 + (Ymesh(1:(Nrow-1),1:(Ncol-1)) - Ymesh(2:Nrow,2:Ncol)).^2 + (Zmesh(1:(Nrow-1),1:(Ncol-1)) - Zmesh(2:Nrow,2:Ncol)).^2 ; + +Div1n = Div1 | ((Dm1 <= Dm2)&Ambi); +Div2n = Div2 | ((Dm2 < Dm1)&Ambi); + +Div11 = Div1n & F1; +Div12 = Div1n & F2; +Div21 = Div2n & F3; +Div22 = Div2n & F4; + + +% look at local difference: + +dZ_r = abs(Zmesh(:,2:Ncol)-Zmesh(:,1:(Ncol-1))); +dZ_c = abs(Zmesh(2:Nrow,:)-Zmesh(1:(Nrow-1),:)); + +Div11 = Div11 & (dZ_r(1:(Nrow-1),:) 1) & (is < Nrow) & (js > 1) & (js < Ncol)); + + is = is(indd); + js = js(indd); + + sm = is + (js-1)*(Nrow); + sm_t = is + (js-1)*(Nrow) - 1; + sm_b = is + (js-1)*(Nrow) + 1; + sm_r = is + (js)*(Nrow); + sm_l = is + (js-2)*(Nrow); + + sm_tr = is + (js)*(Nrow) - 1; + sm_br = is + (js)*(Nrow) + 1; + sm_tl = is + (js-2)*(Nrow) -1; + sm_bl = is + (js-2)*(Nrow) + 1; + + + XX(sm) = 0.5 * XX(sm) + 0.5 * ((XX(sm_t).*t(sm)+XX(sm_b).*b(sm)+XX(sm_r).*r(sm)+XX(sm_l).*l(sm)+XX(sm_tr).*tr(sm)+XX(sm_br).*br(sm)+XX(sm_tl).*tl(sm)+XX(sm_bl).*bl(sm))./Nn(sm)); + + YY(sm) = 0.5 * YY(sm) + 0.5 * ((YY(sm_t).*t(sm)+YY(sm_b).*b(sm)+YY(sm_r).*r(sm)+YY(sm_l).*l(sm)+YY(sm_tr).*tr(sm)+YY(sm_br).*br(sm)+YY(sm_tl).*tl(sm)+YY(sm_bl).*bl(sm))./Nn(sm)); + + ZZ(sm) = 0.5 * ZZ(sm) + 0.5 * ((ZZ(sm_t).*t(sm)+ZZ(sm_b).*b(sm)+ZZ(sm_r).*r(sm)+ZZ(sm_l).*l(sm)+ZZ(sm_tr).*tr(sm)+ZZ(sm_br).*br(sm)+ZZ(sm_tl).*tl(sm)+ZZ(sm_bl).*bl(sm))./Nn(sm)); + + Xmesh = XX(1:Nrow,1:Ncol); + Ymesh = YY(1:Nrow,1:Ncol); + Zmesh = ZZ(1:Nrow,1:Ncol); + + + %%% reconnect after smoothing: + + % diagonal measure: + + Dm1 =( Xmesh(2:Nrow,1:(Ncol-1)) - Xmesh(1:(Nrow-1),2:Ncol)).^2 + (Ymesh(2:Nrow,1:(Ncol-1)) - Ymesh(1:(Nrow-1),2:Ncol)).^2 + (Zmesh(2:Nrow,1:(Ncol-1)) - Zmesh(1:(Nrow-1),2:Ncol)).^2; + + Dm2 = (Xmesh(1:(Nrow-1),1:(Ncol-1)) - Xmesh(2:Nrow,2:Ncol)).^2 + (Ymesh(1:(Nrow-1),1:(Ncol-1)) - Ymesh(2:Nrow,2:Ncol)).^2 + (Zmesh(1:(Nrow-1),1:(Ncol-1)) - Zmesh(2:Nrow,2:Ncol)).^2 ; + + + Div1n = Div1 | ((Dm1 <= Dm2)&Ambi); + Div2n = Div2 | ((Dm2 < Dm1)&Ambi); + + Div11 = Div1n & F1; + Div12 = Div1n & F2; + Div21 = Div2n & F3; + Div22 = Div2n & F4; + + + % look at local difference: + + dZ_r = abs(Zmesh(:,2:Ncol)-Zmesh(:,1:(Ncol-1))); + dZ_c = abs(Zmesh(2:Nrow,:)-Zmesh(1:(Nrow-1),:)); + + Div11 = Div11 & (dZ_r(1:(Nrow-1),:) n_ima, + active_images = active_images(1:n_ima); + end; +end; + +ind_active = find(active_images); + +% I did not call check_active_images, because I want to prevent a break +%check_active_images; + + +fprintf(1,'\nThis function is useful to select a subset of images to calibrate\n'); + + fprintf(1,'\nThere are currently %d active images selected for calibration (out of %d):\n',length(ind_active),n_ima); + + if ~isempty(ind_active), + + if length(ind_active) > 2, + + for ii = 1:length(ind_active)-2, + + fprintf(1,'%d, ',ind_active(ii)); + + end; + + fprintf(1,'%d and %d.',ind_active(end-1),ind_active(end)); + + else + + if length(ind_active) == 2, + + fprintf(1,'%d and %d.',ind_active(end-1),ind_active(end)); + + else + + fprintf(1,'%d.',ind_active(end)); + + end; + + + end; + + end; + + + fprintf(1,'\n'); + + if length(ind_active)==0, + fprintf(1,'\nYou probably want to add images\n'); + choice = 1; + else + if length(ind_active)==n_ima, + fprintf(1,'\nYou probably want to suppress images\n'); + choice = 0; + else + choice = 2; + end; + end; + + if (choice~=0) & (choice ~=1), + fprintf(1,'\nDo you want to suppress or add images from that list?\n'); + end; + +while (choice~=0)&(choice~=1), + choice = input('For suppressing images enter 0, for adding images enter 1 ([]=no change): '); + if isempty(choice), + fprintf(1,'No change applied to the list of active images.\n'); + return; + end; + if (choice~=0)&(choice~=1), + disp('Bad entry. Try again.'); + end; +end; + + +if choice, + + ima_numbers = input('Number(s) of image(s) to add ([] = all images) = '); + +if isempty(ima_numbers), + fprintf(1,'All %d images are now active\n',n_ima); + ima_proc = 1:n_ima; + else + ima_proc = ima_numbers; + end; + +else + + + ima_numbers = input('Number(s) of image(s) to suppress ([] = no image) = '); + + if isempty(ima_numbers), + fprintf(1,'No image has been suppressed. No modication of the list of active images.\n',n_ima); + ima_proc = []; + else + ima_proc = ima_numbers; + end; + +end; + +if ~isempty(ima_proc), + + active_images(ima_proc) = choice * ones(1,length(ima_proc)); + +end; + + + check_active_images; + + + fprintf(1,'\nThere is now a total of %d active images for calibration:\n',length(ind_active)); + + if ~isempty(ind_active), + + if length(ind_active) > 2, + + for ii = 1:length(ind_active)-2, + + fprintf(1,'%d, ',ind_active(ii)); + + end; + + fprintf(1,'%d and %d.',ind_active(end-1),ind_active(end)); + + else + + if length(ind_active) == 2, + + fprintf(1,'%d and %d.',ind_active(end-1),ind_active(end)); + + else + + fprintf(1,'%d.',ind_active(end)); + + end; + + + end; + + end; + + + fprintf(1,'\n\nYou may now run ''Calibration'' to recalibrate based on this new set of images.\n'); + + + \ No newline at end of file diff --git a/src/g_calib/affine.m b/src/g_calib/affine.m new file mode 100755 index 0000000000000000000000000000000000000000..c51b3d176a4a8edf24b62e37e7f0168737503a93 --- /dev/null +++ b/src/g_calib/affine.m @@ -0,0 +1,33 @@ +function A = affine(X,n,om,T,fc,cc,alpha_c) + +if nargin < 7, + alpha_c = 0; + if nargin < 6, + cc = [0;0]; + if nargin < 5, + fc = [1;1]; + if nargin < 4, + T = zeros(3,1); + if nargin < 3, + om = zeros(3,1); + if nargin < 2, + n = [0;0;-1]; + if nargin < 1, + error('Error: affine needs some arguments: A = affine(X,n,om,T,fc,cc,alpha_c);'); + end; + end; + end; + end; + end; + end; +end; + + +KK = [fc(1) alpha_c*fc(1) cc(1); 0 fc(2) cc(2);0 0 1]; +R = rodrigues(om); +omega = n / dot(n,X); +x = X/X(3); + +H = KK * [R + T*omega'] * inv(KK); + +A = (H(3,3)*H(1:2,1:2) - H(1:2,3)*H(3,1:2) + (H(3,2)*H(1:2,1) - H(3,1)*H(1:2,2))*[x(2) -x(1)])/(H(3,:)*x)^2; diff --git a/src/g_calib/align_structures.m b/src/g_calib/align_structures.m new file mode 100755 index 0000000000000000000000000000000000000000..b5b02d6bc7afbb2f86225b11b748e6d27615eede --- /dev/null +++ b/src/g_calib/align_structures.m @@ -0,0 +1,40 @@ +function [om,T,Y] = align_structures(X1,X2), + +% Find om (R) and T, such that Y = R*X1 + T is as close as +% possible to X2. + +[m,n] = size(X1); + +% initialization: + + + + +% initialize param to no motion: +param = zeros(6,1); +change = 1; + +while change > 1e-6, + + om = param(1:3); + T = param(4:6); + + [Y,dYdom,dYdT] = rigid_motion(X1,om,T); + + J = [dYdom dYdT]; + + err = X2(:) - Y(:); + + param_up = inv(J'*J)*J'*err; + + param = param + param_up; + + change = norm(param_up)/norm(param); + +end; + +om = param(1:3); + +T = param(4:6); + +[Y,dYdom,dYdT] = rigid_motion(X1,om,T); diff --git a/src/g_calib/analyse_error.m b/src/g_calib/analyse_error.m new file mode 100755 index 0000000000000000000000000000000000000000..0299d598341ada3922ec0d11e0f0a62fad3f095c --- /dev/null +++ b/src/g_calib/analyse_error.m @@ -0,0 +1,157 @@ +% Color code for each image: + +if ~exist('n_ima')|~exist('fc'), + fprintf(1,'No calibration data available.\n'); + return; +end; + +check_active_images; + +if n_ima ~=0, +if ~exist(['ex_' num2str(ind_active(1)) ]), + fprintf(1,'Need to calibrate before analysing reprojection error. Maybe need to load Calib_Results.mat file.\n'); + return; +end; +end; + + +%if ~exist('no_grid'), +no_grid = 0; +%end; + +colors = 'brgkcm'; + + +figure(5); + +for kk = 1:n_ima, + if exist(['y_' num2str(kk)]), + if active_images(kk) & eval(['~isnan(y_' num2str(kk) '(1,1))']), + + if ~no_grid, + eval(['XX_kk = X_' num2str(kk) ';']); + N_kk = size(XX_kk,2); + + if ~exist(['n_sq_x_' num2str(kk)]), + no_grid = 1; + end; + + if ~no_grid, + eval(['n_sq_x = n_sq_x_' num2str(kk) ';']); + eval(['n_sq_y = n_sq_y_' num2str(kk) ';']); + if (N_kk ~= ((n_sq_x+1)*(n_sq_y+1))), + no_grid = 1; + end; + end; + end; + + eval(['plot(ex_' num2str(kk) '(1,:),ex_' num2str(kk) '(2,:),''' colors(rem(kk-1,6)+1) '+'');']); + + hold on; + end; + end; +end; + +hold off; +axis('equal'); +if 1, %~no_grid, + title('Reprojection error (in pixel) - To exit: right button'); +else + title('Reprojection error (in pixel)'); +end; +xlabel('x'); +ylabel('y'); + +set(5,'color',[1 1 1]); +set(5,'Name','error','NumberTitle','off'); + +if n_ima == 0, + + text(.5,.5,'No image data available','fontsize',24,'horizontalalignment' ,'center'); + +else + +err_std = std(ex')'; + +fprintf(1,'Pixel error: err = [ %3.5f %3.5f] (all active images)\n\n',err_std); + + +b = 1; + +while b==1, + + [xp,yp,b] = ginput4(1); + + if b==1, + ddd = (ex(1,:)-xp).^2 + (ex(2,:)-yp).^2; + + [mind,indmin] = min(ddd); + + + done = 0; + kk_ima = 1; + while (~done)&(kk_ima<=n_ima), + %fprintf(1,'%d...',kk_ima); + eval(['ex_kk = ex_' num2str(kk_ima) ';']); + sol_kk = find((ex_kk(1,:) == ex(1,indmin))&(ex_kk(2,:) == ex(2,indmin))); + if isempty(sol_kk), + kk_ima = kk_ima + 1; + else + done = 1; + end; + end; + + eval(['x_kk = x_' num2str(kk_ima) ';']); + xpt = x_kk(:,sol_kk); + + if ~no_grid, + + eval(['n_sq_x = n_sq_x_' num2str(kk_ima) ';']); + eval(['n_sq_y = n_sq_y_' num2str(kk_ima) ';']); + + Nx = n_sq_x+1; + Ny = n_sq_y+1; + + y1 = floor((sol_kk-1)./Nx); + x1 = sol_kk - 1 - Nx*y1; %rem(sol_kk-1,Nx); + + y1 = (n_sq_y+1) - y1; + x1 = x1 + 1; + + + fprintf(1,'\n'); + fprintf(1,'Selected image: %d\n',kk_ima); + fprintf(1,'Selected point index: %d\n',sol_kk); + fprintf(1,'Pattern coordinates (in units of (dX,dY)): (X,Y)=(%d,%d)\n',[x1-1 y1-1]); + fprintf(1,'Image coordinates (in pixel): (%3.2f,%3.2f)\n',[xpt']); + fprintf(1,'Pixel error = (%3.5f,%3.5f)\n',[ex(1,indmin) ex(2,indmin)]); + + + else + + fprintf(1,'\n'); + fprintf(1,'Selected image: %d\n',kk_ima); + fprintf(1,'Selected point index: %d\n',sol_kk); + fprintf(1,'Image coordinates (in pixel): (%3.2f,%3.2f)\n',[xpt']); + fprintf(1,'Pixel error = (%3.5f,%3.5f)\n',[ex(1,indmin) ex(2,indmin)]); + + + end; + + + if exist(['wintx_' num2str(kk_ima)]), + + eval(['wintx = wintx_' num2str(kk_ima) ';']); + eval(['winty = winty_' num2str(kk_ima) ';']); + + fprintf(1,'Window size: (wintx,winty) = (%d,%d)\n',[wintx winty]); + end; + + + end; + +end; + +disp('done'); + +end; diff --git a/src/g_calib/anisdiff.m b/src/g_calib/anisdiff.m new file mode 100755 index 0000000000000000000000000000000000000000..1365dd64334e38d7b2ef6350e256cba399df5fc5 --- /dev/null +++ b/src/g_calib/anisdiff.m @@ -0,0 +1,55 @@ +function Is = anisdiff(I,sigI,NIter); + +if nargin < 3, + NIter = 4; + if nargin < 2, + sigI = 10; + end; +end; + +% Function that diffuse an image anisotropially. + +Is = I; + +[ny,nx] = size(I); + +c_n = zeros(ny-2,nx-2); +c_s = zeros(ny-2,nx-2); +c_w = zeros(ny-2,nx-2); +c_e = zeros(ny-2,nx-2); +c_nw = zeros(ny-2,nx-2); +c_ne = zeros(ny-2,nx-2); +c_sw = zeros(ny-2,nx-2); +c_se = zeros(ny-2,nx-2); +c_c = ones(ny-2,nx-2); + + +for k=1:NIter, + + dI_n = Is(2:end-1,2:end-1) - Is(1:end-2,2:end-1); + dI_s = Is(2:end-1,2:end-1) - Is(3:end,2:end-1); + dI_w = Is(2:end-1,2:end-1) - Is(2:end-1,1:end-2); + dI_e = Is(2:end-1,2:end-1) - Is(2:end-1,3:end); + dI_nw = Is(2:end-1,2:end-1) - Is(1:end-2,1:end-2); + dI_ne = Is(2:end-1,2:end-1) - Is(1:end-2,3:end); + dI_sw = Is(2:end-1,2:end-1) - Is(3:end,1:end-2); + dI_se = Is(2:end-1,2:end-1) - Is(3:end,3:end); + + + c_n = exp(-.5*(dI_n/sigI).^2)/8; + c_s = exp(-.5*(dI_s/sigI).^2)/8; + c_w = exp(-.5*(dI_w/sigI).^2)/8; + c_e = exp(-.5*(dI_e/sigI).^2)/8; + c_nw = exp(-.5*(dI_nw/sigI).^2)/16; + c_ne = exp(-.5*(dI_ne/sigI).^2)/16; + c_sw = exp(-.5*(dI_sw/sigI).^2)/16; + c_se = exp(-.5*(dI_se/sigI).^2)/16; + + c_c = 1 - (c_n + c_s + c_w + c_e + c_nw + c_ne + c_sw + c_se); + + + Is(2:end-1,2:end-1) = c_c .* Is(2:end-1,2:end-1) + c_n .* Is(1:end-2,2:end-1) + c_s .* Is(3:end,2:end-1) + ... + c_w .* Is(2:end-1,1:end-2) + c_e .* Is(2:end-1,3:end) + c_nw .* Is(1:end-2,1:end-2) + c_ne .* Is(1:end-2,3:end) + ... + c_sw .* Is(3:end,1:end-2) + c_se .* Is(3:end,3:end); + +end; diff --git a/src/g_calib/apply_distortion.m b/src/g_calib/apply_distortion.m new file mode 100755 index 0000000000000000000000000000000000000000..4842f1a6356c236431e1ff92af5483f0d67a4a2e --- /dev/null +++ b/src/g_calib/apply_distortion.m @@ -0,0 +1,97 @@ +function [xd,dxddk] = apply_distortion(x,k) + + +% Complete the distortion vector if you are using the simple distortion model: +length_k = length(k); +if length_k <5 , + k = [k ; zeros(5-length_k,1)]; +end; + + +[m,n] = size(x); + +% Add distortion: + +r2 = x(1,:).^2 + x(2,:).^2; + +r4 = r2.^2; + +r6 = r2.^3; + + +% Radial distortion: + +cdist = 1 + k(1) * r2 + k(2) * r4 + k(5) * r6; + +if nargout > 1, + dcdistdk = [ r2' r4' zeros(n,2) r6']; +end; + + +xd1 = x .* (ones(2,1)*cdist); + +coeff = (reshape([cdist;cdist],2*n,1)*ones(1,3)); + +if nargout > 1, + dxd1dk = zeros(2*n,5); + dxd1dk(1:2:end,:) = (x(1,:)'*ones(1,5)) .* dcdistdk; + dxd1dk(2:2:end,:) = (x(2,:)'*ones(1,5)) .* dcdistdk; +end; + + +% tangential distortion: + +a1 = 2.*x(1,:).*x(2,:); +a2 = r2 + 2*x(1,:).^2; +a3 = r2 + 2*x(2,:).^2; + +delta_x = [k(3)*a1 + k(4)*a2 ; + k(3) * a3 + k(4)*a1]; + +aa = (2*k(3)*x(2,:)+6*k(4)*x(1,:))'*ones(1,3); +bb = (2*k(3)*x(1,:)+2*k(4)*x(2,:))'*ones(1,3); +cc = (6*k(3)*x(2,:)+2*k(4)*x(1,:))'*ones(1,3); + +if nargout > 1, + ddelta_xdk = zeros(2*n,5); + ddelta_xdk(1:2:end,3) = a1'; + ddelta_xdk(1:2:end,4) = a2'; + ddelta_xdk(2:2:end,3) = a3'; + ddelta_xdk(2:2:end,4) = a1'; +end; + +xd = xd1 + delta_x; + +if nargout > 1, + dxddk = dxd1dk + ddelta_xdk ; + if length_k < 5, + dxddk = dxddk(:,1:length_k); + end; +end; + + +return; + +% Test of the Jacobians: + +n = 10; + +lk = 1; + +x = 10*randn(2,n); +k = 0.5*randn(lk,1); + +[xd,dxddk] = apply_distortion(x,k); + + +% Test on k: OK!! + +dk = 0.001 * norm(k)*randn(lk,1); +k2 = k + dk; + +[x2] = apply_distortion(x,k2); + +x_pred = xd + reshape(dxddk * dk,2,n); + + +norm(x2-xd)/norm(x2 - x_pred) diff --git a/src/g_calib/apply_distortion2.m b/src/g_calib/apply_distortion2.m new file mode 100755 index 0000000000000000000000000000000000000000..6061f569cf2d0945127479a181b7144372b5e87a --- /dev/null +++ b/src/g_calib/apply_distortion2.m @@ -0,0 +1,106 @@ +function [xd,dxddk,dxddx] = apply_distortion2(x,k) + + +[m,n] = size(x); + +% Add distortion: + +r2 = x(1,:).^2 + x(2,:).^2; + +r4 = r2.^2; + +r6 = r2.^3; + + +% Radial distortion: + +cdist = 1 + k(1) * r2 + k(2) * r4 + k(5) * r6; + +if nargout > 1, + dcdistdk = [ r2' r4' zeros(n,2) r6']; +end; + + +xd1 = x .* (ones(2,1)*cdist); + +coeff = (reshape([cdist;cdist],2*n,1)*ones(1,3)); + +if nargout > 1, + dxd1dk = zeros(2*n,5); + dxd1dk(1:2:end,:) = (x(1,:)'*ones(1,5)) .* dcdistdk; + dxd1dk(2:2:end,:) = (x(2,:)'*ones(1,5)) .* dcdistdk; +end; + + +% tangential distortion: + +a1 = 2.*x(1,:).*x(2,:); +a2 = r2 + 2*x(1,:).^2; +a3 = r2 + 2*x(2,:).^2; + +delta_x = [k(3)*a1 + k(4)*a2 ; + k(3) * a3 + k(4)*a1]; + +aa = (2*k(3)*x(2,:)+6*k(4)*x(1,:))'*ones(1,3); +bb = (2*k(3)*x(1,:)+2*k(4)*x(2,:))'*ones(1,3); +cc = (6*k(3)*x(2,:)+2*k(4)*x(1,:))'*ones(1,3); + +if nargout > 1, + ddelta_xdk = zeros(2*n,5); + ddelta_xdk(1:2:end,3) = a1'; + ddelta_xdk(1:2:end,4) = a2'; + ddelta_xdk(2:2:end,3) = a3'; + ddelta_xdk(2:2:end,4) = a1'; +end; + +xd = xd1 + delta_x; + +if nargout > 1, + dxddk = dxd1dk + ddelta_xdk ; +end; + +if nargout > 2, + d1 = 1 + k(1).*a2' + k(2).*r2'.*(a2+2*x(1,:).^2)' + k(5).*r4'.*(a2+4*x(1,:).^2)' + 2*k(3).*x(2,:)' + 6*k(4).*x(1,:)'; + d2 = 1 + k(1).*a3' + k(2).*r2'.*(a3+2*x(2,:).^2)' + k(5).*r4'.*(a3+4*x(2,:).^2)' + 6*k(3).*x(2,:)' + 2*k(4).*x(1,:)'; + d3 = (k(1) + 2*k(2).*r2' + 3*k(5).*r4').*a1' + 2*k(3).*x(1,:)' + 2*k(4).*x(2,:)'; + + i = [1:2:2*n 1:2:2*n 2:2:2*n 2:2:2*n]; + j = [1:2:2*n 2:2:2*n 1:2:2*n 2:2:2*n]; + s = [d1' d3' d3' d2']; + + dxddx = sparse(i, j, s, 2*n, 2*n); +end + +return; + +% Test of the Jacobians: + +n = 10; + +x = 10*randn(2,n); +k = 0.5*randn(5,1); + +[xd,dxddk,dxddx] = apply_distortion2(x,k); + + +% Test on k: OK!! + +dk = 0.001 * norm(k)*randn(5,1); +k2 = k + dk; + +[x2] = apply_distortion2(x,k2); + +x_pred = xd + reshape(dxddk * dk,2,n); + + +norm(x2-xd)/norm(x2 - x_pred) + +% Test on x: +dx = 0.000001 *randn(2,n); +x2 = x + dx; + +xd2 = apply_distortion2(x2,k); + +x_pred = xd + reshape(dxddx*dx(:),2,n); + +norm(xd2-xd)/norm(xd2-x_pred) \ No newline at end of file diff --git a/src/g_calib/apply_fisheye_distortion.m b/src/g_calib/apply_fisheye_distortion.m new file mode 100755 index 0000000000000000000000000000000000000000..587ac40e09866e468042c0a58d516e82f38e9053 --- /dev/null +++ b/src/g_calib/apply_fisheye_distortion.m @@ -0,0 +1,25 @@ +function [xd] = apply_fisheye_distortion(x,k) + +%apply_fisheye_distortion.m +% +%[x] = apply_fisheye_distortion(xd,k) +% +%Apply the fisheye distortions +% +%INPUT: x: undistorted (normalized) point coordinates in the image plane (2xN matrix) +% k: Fisheye distortion coefficients (5x1 vector) +% +%OUTPUT: xd: distorted (normalized) point coordinates in the image plane (2xN matrix) + +r = sqrt(x(1,:).^2 + x(2,:).^2); + +theta = atan(r); +theta_d = theta .* (1 + k(1)*theta.^2 + k(2)*theta.^4 + k(3)*theta.^6 + k(4)*theta.^8); + +scaling = ones(1,length(r)); + +ind_good = find(r > 1e-8); + +scaling(ind_good) = theta_d(ind_good) ./ r(ind_good); + +xd = x .* (ones(2,1)*scaling); diff --git a/src/g_calib/calib.m b/src/g_calib/calib.m new file mode 100755 index 0000000000000000000000000000000000000000..9be1481f2cdd44f370111aa8b7b1cae4e34d9aee --- /dev/null +++ b/src/g_calib/calib.m @@ -0,0 +1,18 @@ +function calib(mode), + +% calib(mode) +% +% Runs the Camera Calibration Toolbox. +% Set mode to 1 to run the memory efficient version. +% Any other value for mode will run the normal version (see documentation) + + +if nargin < 1, + + calib_gui; + +else + + calib_gui(mode); + +end; \ No newline at end of file diff --git a/src/g_calib/calib_gui.m b/src/g_calib/calib_gui.m new file mode 100755 index 0000000000000000000000000000000000000000..ec04af5b112fb4ee674320ee01893ef7772f7f37 --- /dev/null +++ b/src/g_calib/calib_gui.m @@ -0,0 +1,29 @@ +%function calib_gui(mode) + +% calib_gui(mode) +% +% Runs the Camera Calibration Toolbox. +% Set mode to 1 to run the memory efficient version. +% Any other value for mode will run the normal version (see documentation) +% +% INFORMATION ABOUT THE MEMORY EFFICIENT MODE FOR THE CAMERA CALIBRATION TOOLBOX: +% +% If your calibration images are large, or if you calibrate using a lot of images, you may have experienced memory problems +% in Matlab when using the calibration toolbox (OUT OF MEMORY errors). If this is the case, you can now run the +% new memory efficient version of the toolbox that loads every image one by one without storing them all in memory. +% If you choose to run the standard version of the toolbox now, you can always switch to the other memory efficient mode +% later in case the OUT OF MEMORY error message is encountered. The two modes of operation are totally compatible. + + +cell_list = {}; + +fig_number = 1; + +title_figure = 'Camera Calibration Toolbox - Select mode of operation:'; + +cell_list{1,1} = {'Standard (all the images are stored in memory)','calib_gui_normal;'}; +cell_list{2,1} = {'Memory efficient (the images are loaded one by one)','calib_gui_no_read;'}; +cell_list{3,1} = {'Exit',['disp(''Bye. To run again, type calib_gui.''); close(' num2str(fig_number) ');']}; + + +show_window(cell_list,fig_number,title_figure,290,18,0,'clean',12); \ No newline at end of file diff --git a/src/g_calib/calib_gui_fisheye.m b/src/g_calib/calib_gui_fisheye.m new file mode 100755 index 0000000000000000000000000000000000000000..68485ef4e640c18265a89e1f856c1e0135264084 --- /dev/null +++ b/src/g_calib/calib_gui_fisheye.m @@ -0,0 +1,39 @@ +%function calib_gui_no_read, + + +cell_list = {}; + + +%-------- Begin editable region -------------% +%-------- Begin editable region -------------% + + +fig_number = 1; + +title_figure = 'Fisheye Camera Calibration Toolbox'; + +cell_list{1,1} = {'Image names','data_calib_no_read;'}; +cell_list{1,2} = {'Read images','ima_read_calib_no_read;'}; +cell_list{1,3} = {'Extract grid corners','click_calib_fisheye_no_read;'}; +cell_list{1,4} = {'Calibration','go_calib_optim_fisheye_no_read;'}; +cell_list{2,1} = {'Show Extrinsic','ext_calib;'}; +cell_list{2,2} = {'Reproject on images','reproject_calib_no_read;'}; +cell_list{2,3} = {'Analyse error','analyse_error;'}; +cell_list{2,4} = {'Recomp. corners','recomp_corner_calib_fisheye_no_read;'}; %recomp_corner_calib_no_read;'}; +cell_list{3,1} = {'Add/Suppress images','add_suppress;'}; +cell_list{3,2} = {'Save','saving_calib_fisheye;'}; +cell_list{3,3} = {'Load','loading_calib;'}; +cell_list{3,4} = {'Exit',['disp(''Bye. To run again, type calib_gui.''); close(' num2str(fig_number) ');']}; %{'Exit','calib_gui;'}; +cell_list{4,1} = {'Comp. Extrinsic','extrinsic_computation;'}; +cell_list{4,2} = {'Undistort image','undistort_image_no_read;'}; +cell_list{4,3} = {'Export calib data','export_calib_data;'}; +cell_list{4,4} = {'Show calib results','show_calib_results_fisheye;'}; +%cell_list{5,1} = {'Smooth images','smooth_images;'}; + +show_window(cell_list,fig_number,title_figure,130,18,0,'clean',12); + + +%-------- End editable region -------------% +%-------- End editable region -------------% + + diff --git a/src/g_calib/calib_gui_no_read.m b/src/g_calib/calib_gui_no_read.m new file mode 100755 index 0000000000000000000000000000000000000000..ba8d354bd96f758e98f0ee0a13074b11aa486a68 --- /dev/null +++ b/src/g_calib/calib_gui_no_read.m @@ -0,0 +1,37 @@ +%function calib_gui_no_read, + + +cell_list = {}; + + +%-------- Begin editable region -------------% +%-------- Begin editable region -------------% + + +fig_number = 1; + +title_figure = 'Camera Calibration Toolbox - Memory efficient version'; + +cell_list{1,1} = {'Image names','data_calib_no_read;'}; +cell_list{1,2} = {'Read images','ima_read_calib_no_read;'}; +cell_list{1,3} = {'Extract grid corners','click_calib_no_read;'}; +cell_list{1,4} = {'Calibration','go_calib_optim_no_read;'}; +cell_list{2,1} = {'Show Extrinsic','ext_calib;'}; +cell_list{2,2} = {'Reproject on images','reproject_calib_no_read;'}; +cell_list{2,3} = {'Analyse error','analyse_error;'}; +cell_list{2,4} = {'Recomp. corners','recomp_corner_calib_no_read;'}; +cell_list{3,1} = {'Add/Suppress images','add_suppress;'}; +cell_list{3,2} = {'Save','saving_calib;'}; +cell_list{3,3} = {'Load','loading_calib;'}; +cell_list{3,4} = {'Exit',['disp(''Bye. To run again, type calib_gui.''); close(' num2str(fig_number) ');']}; %{'Exit','calib_gui;'}; +cell_list{4,1} = {'Comp. Extrinsic','extrinsic_computation;'}; +cell_list{4,2} = {'Undistort image','undistort_image_no_read;'}; +cell_list{4,3} = {'Export calib data','export_calib_data;'}; +cell_list{4,4} = {'Show calib results','show_calib_results;'}; +%cell_list{5,1} = {'Smooth images','smooth_images;'}; + +show_window(cell_list,fig_number,title_figure,130,18,0,'clean',12); + + +%-------- End editable region -------------% +%-------- End editable region -------------% diff --git a/src/g_calib/calib_gui_normal.m b/src/g_calib/calib_gui_normal.m new file mode 100755 index 0000000000000000000000000000000000000000..5c524982da391eafecee5dfc046b994bc06d29fa --- /dev/null +++ b/src/g_calib/calib_gui_normal.m @@ -0,0 +1,43 @@ +%function calib_gui_normal, + + +cell_list = {}; + + +%-------- Begin editable region -------------% +%-------- Begin editable region -------------% + + +fig_number = 1; + +title_figure = 'Camera Calibration Toolbox - Standard Version'; + +clear fc cc kc KK +kc = zeros(5,1); +clearwin; + +cell_list{1,1} = {'Image names','data_calib;'}; +cell_list{1,2} = {'Read images','ima_read_calib;'}; +cell_list{1,3} = {'Extract grid corners','click_calib;'}; +cell_list{1,4} = {'Calibration','go_calib_optim;'}; +cell_list{2,1} = {'Show Extrinsic','ext_calib;'}; +cell_list{2,2} = {'Reproject on images','reproject_calib;'}; +cell_list{2,3} = {'Analyse error','analyse_error;'}; +cell_list{2,4} = {'Recomp. corners','recomp_corner_calib;'}; +cell_list{3,1} = {'Add/Suppress images','add_suppress;'}; +cell_list{3,2} = {'Save','saving_calib;'}; +cell_list{3,3} = {'Load','loading_calib;'}; +cell_list{3,4} = {'Exit',['disp(''Bye. To run again, type calib_gui.''); close(' num2str(fig_number) ');']}; %{'Exit','calib_gui;'}; +cell_list{4,1} = {'Comp. Extrinsic','extrinsic_computation;'}; +cell_list{4,2} = {'Undistort image','undistort_image;'}; +cell_list{4,3} = {'Export calib data','export_calib_data;'}; +cell_list{4,4} = {'Show calib results','show_calib_results;'}; +%cell_list{5,1} = {'Smooth images','smooth_images;'}; + + +show_window(cell_list,fig_number,title_figure,130,18,0,'clean',12); + + +%-------- End editable region -------------% +%-------- End editable region -------------% + diff --git a/src/g_calib/calib_stereo.m b/src/g_calib/calib_stereo.m new file mode 100755 index 0000000000000000000000000000000000000000..e37675178aa997a86a040f10985a990a01c0ab77 --- /dev/null +++ b/src/g_calib/calib_stereo.m @@ -0,0 +1,541 @@ +% calib_stereo +% Script for Calibrating a stereo rig (two cameras, internal and external calibration): +% +% It is assumed that the two cameras (left and right) have been calibrated with the pattern at the same 3D locations, and the same points +% on the pattern (select the same grid points). Therefore, in particular, the same number of images were used to calibrate both cameras. +% The two calibration result files must have been saved under Calib_Results_left.mat and Calib_Results_right.mat prior to running this script. +% This script is not fully documented, therefore use it at your own risks. However, it should be rather straighforward to run. +% +% INPUT: Calib_Results_left.mat, Calib_Results_right.mat -> Generated by the standard calibration toolbox on the two cameras individually) +% OUTPUT: Calib_Results_stereo.mat -> The saved result after global stereo calibration +% +% Main result variables stored in Calib_Results_stereo.mat: +% om, R, T: relative rotation and translation of the right camera wrt the left camera +% fc_left, cc_left, kc_left, alpha_c_left, KK_left: New intrinsic parameters of the left camera +% fc_right, cc_right, kc_right, alpha_c_right, KK_right: New intrinsic parameters of the right camera +% +% Both sets of intrinsic parameters are equivalent to the classical {fc,cc,kc,alpha_c,KK} described online at: +% http://www.vision.caltech.edu/bouguetj/calib_doc/parameters.html +% +% Note: If you do not want to recompute the intinsic parameters, through stereo calibration you may want to set +% recompute_intrinsic_right and recompute_intrinsic_left to zero. Default: 1 +% +% Definition of the extrinsic parameters: R and om are related through the rodrigues formula (R=rodrigues(om)). +% Consider a point P of coordinates XL and XR in the left and right camera reference frames respectively. +% XL and XR are related to each other through the following rigid motion transformation: +% XR = R * XL + T +% R and T (or equivalently om and T) fully describe the relative displacement of the two cameras. +% +% +% If the Warning message "Disabling view kk - Reason: the left and right images are found inconsistent" is encountered, that probably +% means that for the kkth pair of images, the left and right images are found to have captured the calibration pattern at two +% different locations in space. That means that the two views are not consistent, and therefore cannot be used for stereo calibration. +% When capturing your images, make sure that you do not move the calibration pattern between capturing the left and the right images. +% The pattwern can (and should) be moved in space only between two sets of (left,right) images. +% Another reason for inconsistency is that you selected a different set of points on the pattern when running the separate calibrations +% (leading to the two files Calib_Results_left.mat and Calib_Results_left.mat). Make sure that the same points are selected in the +% two separate calibration. In other words, the points need to correspond. + +% (c) Jean-Yves Bouguet - Intel Corporation +% October 25, 2001 -- Last updated April 12, 2002 + + +fprintf(1,'\n\nStereo Calibration script (for more info, try help calib_stereo)\n\n'); + +if (exist('Calib_Results_left.mat')~=2)|(exist('Calib_Results_right.mat')~=2), + fprintf(1,'Error: Need the left and right calibration files Calib_Results_left.mat and Calib_Results_right.mat to run stereo calibration\n'); + return; +end; + +if ~exist('recompute_intrinsic_right'), + recompute_intrinsic_right = 1; +end; + + +if ~exist('recompute_intrinsic_left'), + recompute_intrinsic_left = 1; +end; + +fprintf(1,'Loading the left camera calibration result file Calib_Results_left.mat...\n'); + +clear calib_name + +load Calib_Results_left; + +fc_left = fc; +cc_left = cc; +kc_left = kc; +alpha_c_left = alpha_c; +KK_left = KK; + +if exist('calib_name'), + calib_name_left = calib_name; + format_image_left = format_image; + type_numbering_left = type_numbering; + image_numbers_left = image_numbers; + N_slots_left = N_slots; +else + calib_name_left = ''; + format_image_left = ''; + type_numbering_left = ''; + image_numbers_left = ''; + N_slots_left = ''; +end; + + +X_left = []; + + +om_left_list = []; +T_left_list = []; + +for kk = 1:n_ima, + + if active_images(kk), + + eval(['Xkk = X_' num2str(kk) ';']); + eval(['omckk = omc_' num2str(kk) ';']); + eval(['Rckk = Rc_' num2str(kk) ';']); + eval(['Tckk = Tc_' num2str(kk) ';']); + + N = size(Xkk,2); + + Xckk = Rckk * Xkk + Tckk*ones(1,N); + + X_left = [X_left Xckk]; + + om_left_list = [om_left_list omckk]; + + T_left_list = [T_left_list Tckk]; + + end; +end; + + + +fprintf(1,'Loading the right camera calibration result file Calib_Results_right.mat...\n'); + +clear calib_name + +load Calib_Results_right; + +fc_right = fc; +cc_right = cc; +kc_right = kc; +alpha_c_right = alpha_c; +KK_right = KK; + +if exist('calib_name'), + calib_name_right = calib_name; + format_image_right = format_image; + type_numbering_right = type_numbering; + image_numbers_right = image_numbers; + N_slots_right = N_slots; +else + calib_name_right = ''; + format_image_right = ''; + type_numbering_right = ''; + image_numbers_right = ''; + N_slots_right = ''; +end; + +X_right = []; + +om_right_list = []; +T_right_list = []; + +for kk = 1:n_ima, + + if active_images(kk), + + eval(['Xkk = X_' num2str(kk) ';']); + eval(['omckk = omc_' num2str(kk) ';']); + eval(['Rckk = Rc_' num2str(kk) ';']); + eval(['Tckk = Tc_' num2str(kk) ';']); + + N = size(Xkk,2); + + Xckk = Rckk * Xkk + Tckk*ones(1,N); + + X_right = [X_right Xckk]; + + om_right_list = [om_right_list omckk]; + T_right_list = [T_right_list Tckk]; + + end; +end; + + + + +om_ref_list = []; +T_ref_list = []; +for ii = 1:length(om_left_list), + % Align the structure from the first view: + R_ref = rodrigues(om_right_list(:,ii)) * rodrigues(om_left_list(:,ii))'; + T_ref = T_right_list(:,ii) - R_ref * T_left_list(:,ii); + om_ref = rodrigues(R_ref); + om_ref_list = [om_ref_list om_ref]; + T_ref_list = [T_ref_list T_ref]; +end; + + +% Robust estimate of the initial value for rotation and translation between the two views: +om = median(om_ref_list')'; +T = median(T_ref_list')'; + + + + +if 0, + figure(10); + plot3(X_right(1,:),X_right(2,:),X_right(3,:),'bo'); + hold on; + [Xr2] = rigid_motion(X_left,om,T); + plot3(Xr2(1,:),Xr2(2,:),Xr2(3,:),'r+'); + hold off; + drawnow; +end; + + +R = rodrigues(om); + + + +% Re-optimize now over all the set of extrinsic unknows (global optimization) and intrinsic parameters: + +load Calib_Results_left; + +for kk = 1:n_ima, + if active_images(kk), + eval(['X_left_' num2str(kk) ' = X_' num2str(kk) ';']); + eval(['x_left_' num2str(kk) ' = x_' num2str(kk) ';']); + eval(['omc_left_' num2str(kk) ' = omc_' num2str(kk) ';']); + eval(['Rc_left_' num2str(kk) ' = Rc_' num2str(kk) ';']); + eval(['Tc_left_' num2str(kk) ' = Tc_' num2str(kk) ';']); + end; +end; + +center_optim_left = center_optim; +est_alpha_left = est_alpha; +est_dist_left = est_dist; +est_fc_left = est_fc; +est_aspect_ratio_left = est_aspect_ratio; +active_images_left = active_images; + + +load Calib_Results_right; + +for kk = 1:n_ima, + if active_images(kk), + eval(['X_right_' num2str(kk) ' = X_' num2str(kk) ';']); + eval(['x_right_' num2str(kk) ' = x_' num2str(kk) ';']); + end; +end; + +center_optim_right = center_optim; +est_alpha_right = est_alpha; +est_dist_right = est_dist; +est_fc_right = est_fc; +est_aspect_ratio_right = est_aspect_ratio; +active_images_right = active_images; + + + + +if ~recompute_intrinsic_left, + fprintf(1,'No recomputation of the intrinsic parameters of the left camera (recompute_intrinsic_left = 0)\n'); + center_optim_left = 0; + est_alpha_left = 0; + est_dist_left = zeros(5,1); + est_fc_left = [0;0]; + est_aspect_ratio_left = 1; % just to fix conflicts +end; + +if ~recompute_intrinsic_right, + fprintf(1,'No recomputation of the intrinsic parameters of the right camera (recompute_intrinsic_left = 0)\n'); + center_optim_right = 0; + est_alpha_right = 0; + est_dist_right = zeros(5,1); + est_fc_right = [0;0]; + est_aspect_ratio_right = 1; % just to fix conflicts +end; + + + + +threshold = 50; %1e10; %50; + +active_images = active_images_left & active_images_right; + +history = []; + +fprintf(1,'\nMain stereo calibration optimization procedure - Number of pairs of images: %d\n',length(find(active_images))); +fprintf(1,'Gradient descent iteration: '); + +for iter = 1:12; + + + fprintf(1,'%d...',iter); + + % Jacobian: + + J = []; + e = []; + + param = [fc_left;cc_left;alpha_c_left;kc_left;fc_right;cc_right;alpha_c_right;kc_right;om;T]; + + + for kk = 1:n_ima, + + if active_images(kk), + + % Project the structure onto the left view: + + eval(['Xckk = X_left_' num2str(kk) ';']); + eval(['omckk = omc_left_' num2str(kk) ';']); + eval(['Tckk = Tc_left_' num2str(kk) ';']); + + eval(['xlkk = x_left_' num2str(kk) ';']); + eval(['xrkk = x_right_' num2str(kk) ';']); + + param = [param;omckk;Tckk]; + + % number of points: + Nckk = size(Xckk,2); + + + Jkk = sparse(4*Nckk,20+(1+n_ima)*6); + ekk = zeros(4*Nckk,1); + + + if ~est_aspect_ratio_left, + [xl,dxldomckk,dxldTckk,dxldfl,dxldcl,dxldkl,dxldalphal] = project_points2(Xckk,omckk,Tckk,fc_left(1),cc_left,kc_left,alpha_c_left); + dxldfl = repmat(dxldfl,[1 2]); + else + [xl,dxldomckk,dxldTckk,dxldfl,dxldcl,dxldkl,dxldalphal] = project_points2(Xckk,omckk,Tckk,fc_left,cc_left,kc_left,alpha_c_left); + end; + + + ekk(1:2*Nckk) = xlkk(:) - xl(:); + + Jkk(1:2*Nckk,6*(kk-1)+7+20:6*(kk-1)+7+2+20) = sparse(dxldomckk); + Jkk(1:2*Nckk,6*(kk-1)+7+3+20:6*(kk-1)+7+5+20) = sparse(dxldTckk); + + Jkk(1:2*Nckk,1:2) = sparse(dxldfl); + Jkk(1:2*Nckk,3:4) = sparse(dxldcl); + Jkk(1:2*Nckk,5) = sparse(dxldalphal); + Jkk(1:2*Nckk,6:10) = sparse(dxldkl); + + + % Project the structure onto the right view: + + [omr,Tr,domrdomckk,domrdTckk,domrdom,domrdT,dTrdomckk,dTrdTckk,dTrdom,dTrdT] = compose_motion(omckk,Tckk,om,T); + + if ~est_aspect_ratio_right, + [xr,dxrdomr,dxrdTr,dxrdfr,dxrdcr,dxrdkr,dxrdalphar] = project_points2(Xckk,omr,Tr,fc_right(1),cc_right,kc_right,alpha_c_right); + dxrdfr = repmat(dxrdfr,[1 2]); + else + [xr,dxrdomr,dxrdTr,dxrdfr,dxrdcr,dxrdkr,dxrdalphar] = project_points2(Xckk,omr,Tr,fc_right,cc_right,kc_right,alpha_c_right); + end; + + + ekk(2*Nckk+1:end) = xrkk(:) - xr(:); + + + dxrdom = dxrdomr * domrdom + dxrdTr * dTrdom; + dxrdT = dxrdomr * domrdT + dxrdTr * dTrdT; + + dxrdomckk = dxrdomr * domrdomckk + dxrdTr * dTrdomckk; + dxrdTckk = dxrdomr * domrdTckk + dxrdTr * dTrdTckk; + + + Jkk(2*Nckk+1:end,1+20:3+20) = sparse(dxrdom); + Jkk(2*Nckk+1:end,4+20:6+20) = sparse(dxrdT); + + + Jkk(2*Nckk+1:end,6*(kk-1)+7+20:6*(kk-1)+7+2+20) = sparse(dxrdomckk); + Jkk(2*Nckk+1:end,6*(kk-1)+7+3+20:6*(kk-1)+7+5+20) = sparse(dxrdTckk); + + Jkk(2*Nckk+1:end,11:12) = sparse(dxrdfr); + Jkk(2*Nckk+1:end,13:14) = sparse(dxrdcr); + Jkk(2*Nckk+1:end,15) = sparse(dxrdalphar); + Jkk(2*Nckk+1:end,16:20) = sparse(dxrdkr); + + + emax = max(abs(ekk)); + + if emax < threshold, + + J = [J;Jkk]; + e = [e;ekk]; + + else + + fprintf(1,'Disabling view %d - Reason: the left and right images are found inconsistent (try help calib_stereo for more information)\n',kk); + + active_images(kk) = 0; + + end; + + else + + param = [param;NaN*ones(6,1)]; + + end; + + end; + + history = [history param]; + + ind_Jac = find([est_fc_left & [1;est_aspect_ratio_left];center_optim_left*ones(2,1);est_alpha_left;est_dist_left;est_fc_right & [1;est_aspect_ratio_right];center_optim_right*ones(2,1);est_alpha_right;est_dist_right;ones(6,1);reshape(ones(6,1)*active_images,6*n_ima,1)]); + + ind_active = find(active_images); + + J = J(:,ind_Jac); + J2 = J'*J; + J2_inv = inv(J2); + + param_update = J2_inv*J'*e; + + + param(ind_Jac) = param(ind_Jac) + param_update; + + fc_left = param(1:2); + cc_left = param(3:4); + alpha_c_left = param(5); + kc_left = param(6:10); + fc_right = param(11:12); + cc_right = param(13:14); + alpha_c_right = param(15); + kc_right = param(16:20); + + + if ~est_aspect_ratio_left, + fc_left(2) = fc_left(1); + end; + if ~est_aspect_ratio_right, + fc_right(2) = fc_right(1); + end; + + + om = param(1+20:3+20); + T = param(4+20:6+20); + + + for kk = 1:n_ima; + + if active_images(kk), + + omckk = param(6*(kk-1)+7+20:6*(kk-1)+7+2+20); + Tckk = param(6*(kk-1)+7+3+20:6*(kk-1)+7+5+20); + + eval(['omc_left_' num2str(kk) ' = omckk;']); + eval(['Tc_left_' num2str(kk) ' = Tckk;']); + + end; + + end; + + +end; + +fprintf(1,'done\n'); + + +history = [history param]; + +inconsistent_images = ~active_images & (active_images_left & active_images_right); + + +sigma_x = std(e(:)); +param_error = zeros(20 + (1+n_ima)*6,1); +param_error(ind_Jac) = 3*sqrt(full(diag(J2_inv)))*sigma_x; + +for kk = 1:n_ima; + + if active_images(kk), + + omckk_error = param_error(6*(kk-1)+7+20:6*(kk-1)+7+2+20); + Tckk = param_error(6*(kk-1)+7+3+20:6*(kk-1)+7+5+20); + + eval(['omc_left_error_' num2str(kk) ' = omckk;']); + eval(['Tc_left_error_' num2str(kk) ' = Tckk;']); + + else + + eval(['omc_left_' num2str(kk) ' = NaN*ones(3,1);']); + eval(['Tc_left_' num2str(kk) ' = NaN*ones(3,1);']); + eval(['omc_left_error_' num2str(kk) ' = NaN*ones(3,1);']); + eval(['Tc_left_error_' num2str(kk) ' = NaN*ones(3,1);']); + + end; + +end; + +fc_left_error = param_error(1:2); +cc_left_error = param_error(3:4); +alpha_c_left_error = param_error(5); +kc_left_error = param_error(6:10); +fc_right_error = param_error(11:12); +cc_right_error = param_error(13:14); +alpha_c_right_error = param_error(15); +kc_right_error = param_error(16:20); + +if ~est_aspect_ratio_left, + fc_left_error(2) = fc_left_error(1); +end; +if ~est_aspect_ratio_right, + fc_right_error(2) = fc_right_error(1); +end; + + +om_error = param_error(1+20:3+20); +T_error = param_error(4+20:6+20); + + +KK_left = [fc_left(1) fc_left(1)*alpha_c_left cc_left(1);0 fc_left(2) cc_left(2); 0 0 1]; +KK_right = [fc_right(1) fc_right(1)*alpha_c_right cc_right(1);0 fc_right(2) cc_right(2); 0 0 1]; + + +R = rodrigues(om); + + + +fprintf(1,'\n\nIntrinsic parameters of left camera:\n\n'); +fprintf(1,'Focal Length: fc_left = [ %3.5f %3.5f ] ± [ %3.5f %3.5f ]\n',[fc_left;fc_left_error]); +fprintf(1,'Principal point: cc_left = [ %3.5f %3.5f ] ± [ %3.5f %3.5f ]\n',[cc_left;cc_left_error]); +fprintf(1,'Skew: alpha_c_left = [ %3.5f ] ± [ %3.5f ] => angle of pixel axes = %3.5f ± %3.5f degrees\n',[alpha_c_left;alpha_c_left_error],90 - atan(alpha_c_left)*180/pi,atan(alpha_c_left_error)*180/pi); +fprintf(1,'Distortion: kc_left = [ %3.5f %3.5f %3.5f %3.5f %5.5f ] ± [ %3.5f %3.5f %3.5f %3.5f %5.5f ]\n',[kc_left;kc_left_error]); + + +fprintf(1,'\n\nIntrinsic parameters of right camera:\n\n'); +fprintf(1,'Focal Length: fc_right = [ %3.5f %3.5f ] ± [ %3.5f %3.5f ]\n',[fc_right;fc_right_error]); +fprintf(1,'Principal point: cc_right = [ %3.5f %3.5f ] ± [ %3.5f %3.5f ]\n',[cc_right;cc_right_error]); +fprintf(1,'Skew: alpha_c_right = [ %3.5f ] ± [ %3.5f ] => angle of pixel axes = %3.5f ± %3.5f degrees\n',[alpha_c_right;alpha_c_right_error],90 - atan(alpha_c_right)*180/pi,atan(alpha_c_right_error)*180/pi); +fprintf(1,'Distortion: kc_right = [ %3.5f %3.5f %3.5f %3.5f %5.5f ] ± [ %3.5f %3.5f %3.5f %3.5f %5.5f ]\n',[kc_right;kc_right_error]); + + +fprintf(1,'\n\nExtrinsic parameters (position of right camera wrt left camera):\n\n'); +fprintf(1,'Rotation vector: om = [ %3.5f %3.5f %3.5f ] ± [ %3.5f %3.5f %3.5f ]\n',[om;om_error]); +fprintf(1,'Translation vector: T = [ %3.5f %3.5f %3.5f ] ± [ %3.5f %3.5f %3.5f ]\n',[T;T_error]); + + +fprintf(1,'\n\nNote: The numerical errors are approximately three times the standard deviations (for reference).\n\n') + + +fprintf(1,'Saving the stereo calibration results in Calib_Results_stereo.mat\n\n'); + +string_save = 'save Calib_Results_stereo om R T recompute_intrinsic_right recompute_intrinsic_left calib_name_left format_image_left type_numbering_left image_numbers_left N_slots_left calib_name_right format_image_right type_numbering_right image_numbers_right N_slots_right fc_left cc_left kc_left alpha_c_left KK_left fc_right cc_right kc_right alpha_c_right KK_right active_images dX dY nx ny n_ima active_images_right active_images_left inconsistent_images center_optim_left est_alpha_left est_dist_left est_fc_left est_aspect_ratio_left center_optim_right est_alpha_right est_dist_right est_fc_right est_aspect_ratio_right history param param_error sigma_x om_error T_error fc_left_error cc_left_error kc_left_error alpha_c_left_error fc_right_error cc_right_error kc_right_error alpha_c_right_error'; + +for kk = 1:n_ima, + if active_images(kk), + string_save = [string_save ' X_left_' num2str(kk) ' omc_left_' num2str(kk) ' Tc_left_' num2str(kk) ' omc_left_error_' num2str(kk) ' Tc_left_error_' num2str(kk) ' n_sq_x_' num2str(kk) ' n_sq_y_' num2str(kk)]; + end; +end; +eval(string_save); + + +% Plot the extrinsic parameters: +ext_calib_stereo; + diff --git a/src/g_calib/cam_proj_calib.m b/src/g_calib/cam_proj_calib.m new file mode 100755 index 0000000000000000000000000000000000000000..8c34c7706f59e7a4d98eec2321f470cb23ffcbe6 --- /dev/null +++ b/src/g_calib/cam_proj_calib.m @@ -0,0 +1,191 @@ +%%% This code is an additional code that helps doing projector calibration in 3D scanning setup. +%%% This is not a useful code for anyone else but me. +%%% I included it in the toolbox for illustration only. + + +fprintf(1,'3D scanner calibration code\n'); +fprintf(1,'(c) Jean-Yves Bouguet - August 2000\n'); +fprintf(1,'Intel Corporation\n'); + + +if ~exist('camera_results.mat'), + if exist('Calib_Results.mat'), + copyfile('Calib_Results.mat','camera_results.mat'); + delete('Calib_Results.mat'); + else + disp('ERROR: Need to calibrate the camera first, save results, and run cam_proj_calib'); + break; + end; +end; + + + +if 0, % If I want to run camera calibration again + load camera_results; + % Do estimate distortion: + est_dist = [1 0 0 0 0]; %ones(5,1); + est_alpha = 0; + center_optim = 1; + % Run the main calibration routine: + go_calib_optim; + saving_calib; + copyfile('Calib_Results.mat','camera_results.mat'); + delete('Calib_Results.mat'); +end; + + + + +%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% START THE MAIN PROCEDURE %%%%%%%%%%%%%%%%%%%%%%%%%%% + +load camera_results; +param = solution; + +% Save camera parameters: +fc_save = fc; +cc_save = cc; +kc_save = kc; +alpha_c_save = alpha_c; + +omc_1_save = omc_1; +Rc_1_save = Rc_1; +Tc_1_save = Tc_1; + +clear fc cc kc alpha_c + + +param_cam = param([1:10 16:end]); + + +% Extract projector data? +if ~exist('projector_data.mat'), + projector_calib; % extract the projector corners (all the data) +else + load projector_data; % load the projector corners (previously saved) +end; + + + +% Start projector calibration: + +X_proj = []; +x_proj = []; +n_ima_proj = []; + +for kk = ind_active, + eval(['xproj = xproj_' num2str(kk) ';']); + xprojn = normalize_pixel(xproj,fc_save,cc_save,kc_save,alpha_c_save); + eval(['Rc = Rc_' num2str(kk) ';']); + eval(['Tc = Tc_' num2str(kk) ';']); + + Np_proj = size(xproj,2); + Zc = ((Rc(:,3)'*Tc) * (1./(Rc(:,3)' * [xprojn; ones(1,Np_proj)]))); + Xcp = (ones(3,1)*Zc) .* [xprojn; ones(1,Np_proj)]; % % in the camera frame + eval(['X_proj_' num2str(kk) ' = Xcp;']); % coordinates of the points in the + eval(['X_proj = [X_proj X_proj_' num2str(kk) '];']); + eval(['x_proj = [x_proj x_proj_' num2str(kk) '];']); + n_ima_proj = [n_ima_proj kk*ones(1,Np_proj)]; +end; + +% Image size: (may or may not be available) +nx = 1024; +ny = 768; + +% No calibration image is available (only the corner coordinates) +no_image = 1; + +n_ima_save = n_ima; +X_1_save = X_1; +x_1_save = x_1; +dX_save = dX; +dY_save = dY; + +n_ima = 1; +X_1 = X_proj; +x_1 = x_proj; + +% Set the toolbox not to prompt the user (choose default values) +dont_ask = 1; + +% Do estimate distortion: +est_dist = [1 0 0 0 0]'; %ones(5,1); +est_alpha = 0; +center_optim = 1; + + +% Run the main calibration routine: +clear fc kc cc alpha_c KK +go_calib_optim; + +param = solution; + +param_proj = param([1:10 16:end]); + +% Shows the extrinsic parameters: +dX = 30; +dY = 30; +ext_calib; + +% Reprojection on the original images: +reproject_calib; +%saving_calib; +%copyfile([save_name '.mat'],'projector_results.mat'); + + +saving_calib; + +copyfile('Calib_Results.mat','projector_results.mat'); +delete('Calib_Results.mat'); + + +n_ima = n_ima_save; +X_1 = X_1_save; +x_1 = x_1_save; +no_image = 0; +dX = dX_save; +dY = dY_save; + +%----------------------- Retrieve results: + +% Intrinsic: + +% Projector: +fp = fc; +cp = cc; +kp = kc; +alpha_p = alpha_c; + +% Camera: +fc = fc_save; +cc = cc_save; +kc = kc_save; +alpha_c = alpha_c_save; + + +% Extrinsic: + +% Relative position of projector and camera: +T = Tc_1; +om = omc_1; +R = rodrigues(om); + +% Relative prosition of camera wrt world: +omc = omc_1_save; +Rc = Rc_1_save; +Tc = Tc_1_save; + +% relative position of projector wrt world: +Rp = R*Rc; +omp = rodrigues(Rp); +Tp = T + R*Tc; + +eval(['save calib_cam_proj R om T fc fp cc cp alpha_c alpha_p kc kp Rc Rp Tc Tp omc omp']); + + + +% Final refinement: + +%----------------- global optimization: --------------------- + +cam_proj_calib_optim; + diff --git a/src/g_calib/cam_proj_calib_optim.m b/src/g_calib/cam_proj_calib_optim.m new file mode 100755 index 0000000000000000000000000000000000000000..729f1d5e0ce3aa3927bdba0ae228f5e9b2680241 --- /dev/null +++ b/src/g_calib/cam_proj_calib_optim.m @@ -0,0 +1,122 @@ +load camera_results; + +string_global = 'global n_ima'; +for kk = 1:n_ima, + string_global = [string_global ' x_' num2str(kk) ' X_' num2str(kk) ' xproj_' num2str(kk) ' x_proj_' num2str(kk)]; +end; +eval(string_global); + + +%----------------- global optimization: --------------------- + +load projector_data; % load the projector corners (previously saved) +load projector_results; + +solution_projector = solution; + + +load camera_results; + +solution_camera = solution; + +param_cam = solution_camera([1:10 16:end]); +param_proj = solution_projector([1:10 16:end]); + +param = [param_cam;param_proj]; + + +% Restart the minimization from here (if need be): +load camera_results; +load calib_cam_proj_optim2; + + +options = [1 1e-4 1e-4 1e-6 0 0 0 0 0 0 0 0 0 12000 0 1e-8 0.1 0]; + +%if 0, % use the full distortion model: + +% fprintf(1,'Take the complete distortion model\n'); + + % test the global error function: +% e_global = error_cam_proj(param); + +% param_init = param; + +% param = leastsq('error_cam_proj',param,options); + + +%else + + % Use a limitd distortion model (no 6th order) + fprintf(1,'Take the 6th order distortion coefficient out\n'); + + param = param([1:9 11:11+6*n_ima-1 11+6*n_ima:11+6*n_ima+9-1 11+6*n_ima+9+1:end]); + + % test the global error function: + e_global2 = error_cam_proj2(param); + + param_init = param; + + param = leastsq('error_cam_proj2',param,options); + + param = [param(1:9);0;param(10:10+6*n_ima-1);param(10+6*n_ima:10+6*n_ima+9-1);0;param(10+6*n_ima+9:end)]; + +%end; + + + + +% Extract the parameters: + +cam_proj_extract_param; + + +% Relative prosition of camera wrt world: +omc = omc_1; +Rc = Rc_1; +Tc = Tc_1; + +% relative position of projector wrt world: +Rp = R*Rc; +omp = rodrigues(Rp); +Tp = T + R*Tc; + +eval(['save calib_cam_proj_optim3 R om T fc fp cc cp alpha_c alpha_p kc kp Rc Rp Tc Tp omc omp param param_init']); + +no_image = 0; +% Image size: (may or may not be available) +nx = 640; +ny = 480; + +comp_error_calib; + +% Save the optimal camera parameters: +saving_calib; +copyfile('Calib_Results.mat','camera_results_optim3.mat'); +delete('Calib_Results.mat'); + +% Save the optimal camera parameters: +fc = fp; +cc = cp; +alpha_c = alpha_p; +kc = kp; + +n_ima = 1; +X_1 = X_proj; +x_1 = x_proj; +omc_1 = om; +Tc_1 = T; +Rc_1 = R; + +% Image size: (may or may not be available) +nx = 1024; +ny = 768; + +% No calibration image is available (only the corner coordinates) +no_image = 1; + +comp_error_calib; + +saving_calib; +copyfile('Calib_Results.mat','projector_results_optim3.mat'); +delete('Calib_Results.mat'); + diff --git a/src/g_calib/cam_proj_extract_param.m b/src/g_calib/cam_proj_extract_param.m new file mode 100755 index 0000000000000000000000000000000000000000..9675c37d6cba0c73f82a0e00ea199d2d1c556400 --- /dev/null +++ b/src/g_calib/cam_proj_extract_param.m @@ -0,0 +1,58 @@ + +% Computation of the errors: + +fc = param(1:2); +cc = param(3:4); +alpha_c = param(5); +kc = param(6:10); + +e_cam = []; + +for kk = 1:n_ima, + omckk = param(11+(kk-1)*6:11+(kk-1)*6+2); + Tckk = param(11+(kk-1)*6+3:11+(kk-1)*6+3+2); + + eval(['Xkk = X_' num2str(kk) ';']); + eval(['xkk = x_' num2str(kk) ';']); + + ekk = xkk - project_points2(Xkk,omckk,Tckk,fc,cc,kc,alpha_c); + + Rckk = rodrigues(omckk); + eval(['omc_' num2str(kk) '= omckk;']); + eval(['Tc_' num2str(kk) '= Tckk;']); + eval(['Rc_' num2str(kk) '= Rckk;']); + + e_cam = [e_cam ekk]; + +end; + +X_proj = []; +x_proj = []; + +for kk = 1:n_ima, + eval(['xproj = xproj_' num2str(kk) ';']); + xprojn = normalize_pixel(xproj,fc,cc,kc,alpha_c); + eval(['Rc = Rc_' num2str(kk) ';']); + eval(['Tc = Tc_' num2str(kk) ';']); + Np_proj = size(xproj,2); + Zc = ((Rc(:,3)'*Tc) * (1./(Rc(:,3)' * [xprojn; ones(1,Np_proj)]))); + Xcp = (ones(3,1)*Zc) .* [xprojn; ones(1,Np_proj)]; % % in the camera frame + eval(['X_proj_' num2str(kk) ' = Xcp;']); % coordinates of the points in the + eval(['X_proj = [X_proj X_proj_' num2str(kk) '];']); + eval(['x_proj = [x_proj x_proj_' num2str(kk) '];']); +end; + + +fp = param((1:2)+n_ima * 6 + 10); +cp = param((3:4)+n_ima * 6 + 10); +alpha_p = param((5)+n_ima * 6 + 10); +kp = param((6:10)+n_ima * 6 + 10); + +om = param(10+n_ima*6+10+1:10+n_ima*6+10+1+2); +T = param(10+n_ima*6+10+1+2+1:10+n_ima*6+10+1+2+1+2); +R = rodrigues(om); + +e_proj = x_proj - project_points2(X_proj,om,T,fp,cp,kp,alpha_p); + +e_global = [e_cam e_proj]; + diff --git a/src/g_calib/centercirclefinder.m b/src/g_calib/centercirclefinder.m new file mode 100755 index 0000000000000000000000000000000000000000..62ff492c42020a09de2e5412d0072ebd27b330a6 --- /dev/null +++ b/src/g_calib/centercirclefinder.m @@ -0,0 +1,173 @@ +function [xc,good,bad] = centercirclefinder(xt,I,wintx,winty); + + +%xt = [521.6249 ; 469.9916]; +%wintx = 60; +%winty = 60; + + +xt = xt'; +xt = fliplr(xt); + + +if nargin < 4, + winty = 5; + if nargin < 3, + wintx = 5; + end; +end; + + +if nargin < 6, + wx2 = -1; + wy2 = -1; +end; + + +mask = ones(2*wintx + 1,2*winty + 1); +%mask = exp(-((-wintx:wintx)'/(wintx)).^2) * exp(-((-winty:winty)/(winty)).^2); + + + +offx = [-wintx:wintx]'*ones(1,2*winty+1); +offy = ones(2*wintx+1,1)*[-winty:winty]; + +resolution = 0.0001; + +MaxIter = 15; + +[nx,ny] = size(I); +N = size(xt,1); + +xc = xt; % first guess... they don't move !!! + +type_feat = zeros(1,N); + + +for i=1:N, + + v_extra = resolution + 1; % just larger than resolution + + compt = 0; % no iteration yet + + while (norm(v_extra) > resolution) & (compt 0, % the sub pixel + vIx = [itIx 1-itIx 0]'; % accuracy. + else + vIx = [0 1+itIx -itIx]'; + end; + if itIy > 0, + vIy = [itIy 1-itIy 0]; + else + vIy = [0 1+itIy -itIy]; + end; + + + % What if the sub image is not in? + + if (crIx-wintx-2 < 1), xmin=1; xmax = 2*wintx+5; + elseif (crIx+wintx+2 > nx), xmax = nx; xmin = nx-2*wintx-4; + else + xmin = crIx-wintx-2; xmax = crIx+wintx+2; + end; + + if (crIy-winty-2 < 1), ymin=1; ymax = 2*winty+5; + elseif (crIy+winty+2 > ny), ymax = ny; ymin = ny-2*winty-4; + else + ymin = crIy-winty-2; ymax = crIy+winty+2; + end; + + + SI = I(xmin:xmax,ymin:ymax); % The necessary neighborhood + SI = conv2(conv2(SI,vIx,'same'),vIy,'same'); + SI = SI(2:2*wintx+4,2:2*winty+4); % The subpixel interpolated neighborhood + [gy,gx] = gradient(SI); % The gradient image + gx = gx(2:2*wintx+2,2:2*winty+2); % extraction of the useful parts only + gy = gy(2:2*wintx+2,2:2*winty+2); % of the gradients + + px = cIx + offx; + py = cIy + offy; + + gxx = gx .* gx .* mask; + gyy = gy .* gy .* mask; + gxy = gx .* gy .* mask; + + + bb = [sum(sum(gyy .* px - gxy .* py)); sum(sum(-gxy .* px + gxx .* py))]; + + a = sum(sum(gyy)); + b = -sum(sum(gxy)); + c = sum(sum(gxx)); + + dt = a*c - b^2; + + xc2 = [c*bb(1)-b*bb(2) a*bb(2)-b*bb(1)]/dt; + + + %keyboard; + + + %keyboard; + +% G = [a b;b c]; +% [U,S,V] = svd(G); + + +% if S(1,1)/S(2,2) > 150, +% bb2 = U'*bb; +% xc2 = (V*[bb2(1)/S(1,1) ;0])'; +% else +% xc2 = [c*bb(1)-b*bb(2) a*bb(2)-b*bb(1)]/dt; +% end; + + + %if (abs(a)> 50*abs(c)), +% xc2 = [(c*bb(1)-b*bb(2))/dt xc(i,2)]; +% elseif (abs(c)> 50*abs(a)) +% xc2 = [xc(i,1) (a*bb(2)-b*bb(1))/dt]; +% else +% xc2 = [c*bb(1)-b*bb(2) a*bb(2)-b*bb(1)]/dt; +% end; + + %keyboard; + + v_extra = xc(i,:) - xc2; + + xc(i,:) = xc2; + +% keyboard; + + compt = compt + 1; + + end +end; + + +% check for points that diverge: + +delta_x = xc(:,1) - xt(:,1); +delta_y = xc(:,2) - xt(:,2); + +%keyboard; + + +bad = (abs(delta_x) > wintx) | (abs(delta_y) > winty); +good = ~bad; +in_bad = find(bad); + +% For the diverged points, keep the original guesses: + +xc(in_bad,:) = xt(in_bad,:); + +xc = fliplr(xc); +xc = xc'; + +bad = bad'; +good = good'; diff --git a/src/g_calib/check_active_images.m b/src/g_calib/check_active_images.m new file mode 100755 index 0000000000000000000000000000000000000000..87c866ed66d87e86d7afbb702fb26628f368efb4 --- /dev/null +++ b/src/g_calib/check_active_images.m @@ -0,0 +1,38 @@ +if n_ima ~= 0, + + if ~exist('active_images'), + active_images = ones(1,n_ima); + end; + n_act = length(active_images); + if n_act < n_ima, + active_images = [active_images ones(1,n_ima-n_act)]; + else + if n_act > n_ima, + active_images = active_images(1:n_ima); + end; + end; + + ind_active = find(active_images); + + if prod(double(active_images == 0)), + disp('Error: There is no active image. Run Add/Suppress images to add images'); + break + end; + + if exist('center_optim'), + center_optim = double(center_optim); + end; + if exist('est_alpha'), + est_alpha = double(est_alpha); + end; + if exist('est_dist'), + est_dist = double(est_dist); + end; + if exist('est_fc'), + est_fc = double(est_fc); + end; + if exist('est_aspect_ratio'), + est_aspect_ratio = double(est_aspect_ratio); + end; + +end; diff --git a/src/g_calib/check_convergence.m b/src/g_calib/check_convergence.m new file mode 100755 index 0000000000000000000000000000000000000000..c4b13fd56ade64ad53f2c3ba18135260d45577d1 --- /dev/null +++ b/src/g_calib/check_convergence.m @@ -0,0 +1,48 @@ +%%% Replay the set of solution vectors: + + +if ~exist('param_list'), + if ~exist('solution'); + fprintf(1,'Error: Need to calibrate first\n'); + return; + else + param_list = solution; + end; +end; + +N_iter = size(param_list,2); + +if N_iter == 1, + fprintf(1,'Warning: There is a unique state in the list of parameters.\n'); +end; + + + +%M = moviein(N_iter); + +for nn = 1:N_iter, + + solution = param_list(:,nn); + + extract_parameters; + comp_error_calib; + + ext_calib; + + drawnow; + +% Mnn = getframe(gcf); + +% M(:,nn) = Mnn; + +end; + +%fig = gcf; + + +%figure(fig+1); +%close; +%figure(fig+1); + +%clf; +%movie(M,20); diff --git a/src/g_calib/check_directory.m b/src/g_calib/check_directory.m new file mode 100755 index 0000000000000000000000000000000000000000..9a9416374b342edffb8494cdf5b95b31cea18f64 --- /dev/null +++ b/src/g_calib/check_directory.m @@ -0,0 +1,190 @@ +% This small script looks in the direcory and checks if the images are there. +% +% This works only on Matlab 5.x (otherwise, the dir commands works differently) + +% (c) Jean-Yves Bouguet - Dec. 27th, 1999 + +l = dir([calib_name '*']); + +Nl = size(l,1); +Nima_valid = 0; +ind_valid = []; +loc_extension = []; +length_name = size(calib_name,2); + +if Nl > 0, + + for pp = 1:Nl, + + filenamepp = l(pp).name; + if isempty(calib_name), + iii = 1; + else + iii = findstr(filenamepp,calib_name); + end; + + loc_ext = findstr(filenamepp,format_image); + string_num = filenamepp(length_name+1:loc_ext - 2); + + if isempty(str2num(string_num)), + iii = []; + end; + + + if ~isempty(iii), + if (iii(1) ~= 1), + iii = []; + end; + end; + + + + if ~isempty(iii) & ~isempty(loc_ext), + + Nima_valid = Nima_valid + 1; + ind_valid = [ind_valid pp]; + loc_extension = [loc_extension loc_ext(1)]; + + end; + + end; + + if (Nima_valid==0), + + % try the upper case format: + + format_image = upper(format_image); + + + + for pp = 1:Nl, + + filenamepp = l(pp).name; + + if isempty(calib_name), + iii = 1; + else + iii = findstr(filenamepp,calib_name); + end; + + loc_ext = findstr(filenamepp,format_image); + string_num = filenamepp(length_name+1:loc_ext - 2); + + if isempty(str2num(string_num)), + iii = []; + end; + + + if ~isempty(iii), + if (iii(1) ~= 1), + iii = []; + end; + end; + + + + if ~isempty(iii) & ~isempty(loc_ext), + + Nima_valid = Nima_valid + 1; + ind_valid = [ind_valid pp]; + loc_extension = [loc_extension loc_ext(1)]; + + end; + + end; + + if (Nima_valid==0), + + fprintf(1,'No image found. File format may be wrong.\n'); + + else + + + % Get all the string numbers: + + string_length = zeros(1,Nima_valid); + indices = zeros(1,Nima_valid); + + + for ppp = 1:Nima_valid, + + name = l(ind_valid(ppp)).name; + string_num = name(length_name+1:loc_extension(ppp) - 2); + string_length(ppp) = size(string_num,2); + indices(ppp) = str2num(string_num); + + end; + + % Number of images... + first_num = min(indices); + n_ima = max(indices) - first_num + 1; + + if min(string_length) == max(string_length), + + N_slots = min(string_length); + type_numbering = 1; + + else + + N_slots = 1; + type_numbering = 0; + + end; + + image_numbers = first_num:n_ima-1+first_num; + + %%% By default, all the images are active for calibration: + + active_images = ones(1,n_ima); + + + + end; + + else + + % Get all the string numbers: + + string_length = zeros(1,Nima_valid); + indices = zeros(1,Nima_valid); + + + for ppp = 1:Nima_valid, + + name = l(ind_valid(ppp)).name; + string_num = name(length_name+1:loc_extension(ppp) - 2); + string_length(ppp) = size(string_num,2); + indices(ppp) = str2num(string_num); + + end; + + % Number of images... + first_num = min(indices); + n_ima = max(indices) - first_num + 1; + + if min(string_length) == max(string_length), + + N_slots = min(string_length); + type_numbering = 1; + + else + + N_slots = 1; + type_numbering = 0; + + end; + + image_numbers = first_num:n_ima-1+first_num; + + %%% By default, all the images are active for calibration: + + active_images = ones(1,n_ima); + + end; + +else + + fprintf(1,'No image found. Basename may be wrong.\n'); + +end; + diff --git a/src/g_calib/check_extracted_images.m b/src/g_calib/check_extracted_images.m new file mode 100755 index 0000000000000000000000000000000000000000..fa7df879e388423817ca0a49fceadbfd9fc4157b --- /dev/null +++ b/src/g_calib/check_extracted_images.m @@ -0,0 +1,37 @@ +check_active_images; + +for kk = ind_active, + + if ~exist(['x_' num2str(kk)]), + + fprintf(1,'WARNING: Need to extract grid corners on image %d\n',kk); + + active_images(kk) = 0; + + eval(['dX_' num2str(kk) ' = NaN;']); + eval(['dY_' num2str(kk) ' = NaN;']); + + eval(['wintx_' num2str(kk) ' = NaN;']); + eval(['winty_' num2str(kk) ' = NaN;']); + + eval(['x_' num2str(kk) ' = NaN*ones(2,1);']); + eval(['X_' num2str(kk) ' = NaN*ones(3,1);']); + + eval(['n_sq_x_' num2str(kk) ' = NaN;']); + eval(['n_sq_y_' num2str(kk) ' = NaN;']); + + else + + eval(['xkk = x_' num2str(kk) ';']); + + if isnan(xkk(1)), + + fprintf(1,'WARNING: Need to extract grid corners on image %d - This image is now set inactive\n',kk); + + active_images(kk) = 0; + + end; + + end; + +end; diff --git a/src/g_calib/clear_windows.m b/src/g_calib/clear_windows.m new file mode 100755 index 0000000000000000000000000000000000000000..1eccbd3d47756bc292a53219282cf2b9d504a026 --- /dev/null +++ b/src/g_calib/clear_windows.m @@ -0,0 +1,4 @@ +for kk = 1:n_ima, + eval(['clear wintx_' num2str(kk)]); + eval(['clear winty_' num2str(kk)]); +end; diff --git a/src/g_calib/clearwin.m b/src/g_calib/clearwin.m new file mode 100755 index 0000000000000000000000000000000000000000..fccc8c45c9a1fede20d9832d56f73ee592721592 --- /dev/null +++ b/src/g_calib/clearwin.m @@ -0,0 +1,14 @@ +% Function that clears all the wintx_i and winty_i +% In normal operation of the toolbox, this function should not be +% useful. +% only in cases where you want to re-extract corners using the Extract grid corners another time... not common. You might as well use the Recomp. corners. + +if exist('n_ima','var')~=1 + return; +end; + +for kk = 1:n_ima, + + eval(['clear wintx_' num2str(kk) ' winty_' num2str(kk)]); + +end; diff --git a/src/g_calib/click_calib.m b/src/g_calib/click_calib.m new file mode 100755 index 0000000000000000000000000000000000000000..41d011f48ea2f653301707f6eb6d0a820c4435ee --- /dev/null +++ b/src/g_calib/click_calib.m @@ -0,0 +1,214 @@ +%if exist('images_read'); +% active_images = active_images & images_read; +%end; + +var2fix = 'dX_default'; + +fixvariable; + +var2fix = 'dY_default'; + +fixvariable; + +var2fix = 'map'; + +fixvariable; + + +if ~exist('n_ima'), + data_calib; +end; + +check_active_images; + +if ~exist(['I_' num2str(ind_active(1))]), + ima_read_calib; + if isempty(ind_read), + disp('Cannot extract corners without images'); + return; + end; +end; + + +fprintf(1,'\nExtraction of the grid corners on the images\n'); + + +if (exist('map')~=1), map = gray(256); end; + + +if exist('dX'), + dX_default = dX; +end; + +if exist('dY'), + dY_default = dY; +end; + +if exist('n_sq_x'), + n_sq_x_default = n_sq_x; +end; + +if exist('n_sq_y'), + n_sq_y_default = n_sq_y; +end; + + +if ~exist('dX_default')|~exist('dY_default'); + + % Setup of JY - 3D calibration rig at Intel (new at Intel) - use units in mm to match Zhang + dX_default = 30; + dY_default = 30; + + % Setup of JY - 3D calibration rig at Google - use units in mm to match Zhang + dX_default = 100; + dY_default = 100; + +end; + + +if ~exist('n_sq_x_default')|~exist('n_sq_y_default'), + n_sq_x_default = 10; + n_sq_y_default = 10; +end; + +if ~exist('wintx_default')|~exist('winty_default'), + wintx_default = max(round(nx/128),round(ny/96)); + winty_default = wintx_default; + clear wintx winty +end; + + +if ~exist('wintx') | ~exist('winty'), + clear_windows; % Clear all the window sizes (to re-initiate) +end; + + + +if ~exist('dont_ask'), + dont_ask = 0; +end; + + +if ~dont_ask, + ima_numbers = input('Number(s) of image(s) to process ([] = all images) = '); +else + ima_numbers = []; +end; + +if isempty(ima_numbers), + ima_proc = 1:n_ima; +else + ima_proc = ima_numbers; +end; + + +% Useful option to add images: +kk_first = ima_proc(1); %input('Start image number ([]=1=first): '); + + +if exist(['wintx_' num2str(kk_first)]), + + eval(['wintxkk = wintx_' num2str(kk_first) ';']); + + if isempty(wintxkk) | isnan(wintxkk), + + disp('Window size for corner finder (wintx and winty):'); + wintx = input(['wintx ([] = ' num2str(wintx_default) ') = ']); + if isempty(wintx), wintx = wintx_default; end; + wintx = round(wintx); + winty = input(['winty ([] = ' num2str(winty_default) ') = ']); + if isempty(winty), winty = winty_default; end; + winty = round(winty); + + fprintf(1,'Window size = %dx%d\n',2*wintx+1,2*winty+1); + + end; + +else + + disp('Window size for corner finder (wintx and winty):'); + wintx = input(['wintx ([] = ' num2str(wintx_default) ') = ']); + if isempty(wintx), wintx = wintx_default; end; + wintx = round(wintx); + winty = input(['winty ([] = ' num2str(winty_default) ') = ']); + if isempty(winty), winty = winty_default; end; + winty = round(winty); + + fprintf(1,'Window size = %dx%d\n',2*wintx+1,2*winty+1); + +end; + + +if ~dont_ask, + fprintf(1,'Do you want to use the automatic square counting mechanism (0=[]=default)\n'); + manual_squares = input(' or do you always want to enter the number of squares manually (1,other)? '); + if isempty(manual_squares), + manual_squares = 0; + else + manual_squares = ~~manual_squares; + end; +else + manual_squares = 0; +end; + + +for kk = ima_proc, + if exist(['I_' num2str(kk)]), + click_ima_calib; + active_images(kk) = 1; + else + eval(['dX_' num2str(kk) ' = NaN;']); + eval(['dY_' num2str(kk) ' = NaN;']); + + eval(['wintx_' num2str(kk) ' = NaN;']); + eval(['winty_' num2str(kk) ' = NaN;']); + + eval(['x_' num2str(kk) ' = NaN*ones(2,1);']); + eval(['X_' num2str(kk) ' = NaN*ones(3,1);']); + + eval(['n_sq_x_' num2str(kk) ' = NaN;']); + eval(['n_sq_y_' num2str(kk) ' = NaN;']); + end; +end; + + +check_active_images; + + + +% Fix potential non-existing variables: + +for kk = 1:n_ima, + if ~exist(['x_' num2str(kk)]), + eval(['dX_' num2str(kk) ' = NaN;']); + eval(['dY_' num2str(kk) ' = NaN;']); + + eval(['x_' num2str(kk) ' = NaN*ones(2,1);']); + eval(['X_' num2str(kk) ' = NaN*ones(3,1);']); + + eval(['n_sq_x_' num2str(kk) ' = NaN;']); + eval(['n_sq_y_' num2str(kk) ' = NaN;']); + end; + + if ~exist(['wintx_' num2str(kk)]) | ~exist(['winty_' num2str(kk)]), + + eval(['wintx_' num2str(kk) ' = NaN;']); + eval(['winty_' num2str(kk) ' = NaN;']); + + end; +end; + +string_save = 'save calib_data active_images ind_active wintx winty n_ima type_numbering N_slots first_num image_numbers format_image calib_name Hcal Wcal nx ny map dX_default dY_default dX dY wintx_default winty_default'; + +for kk = 1:n_ima, + string_save = [string_save ' X_' num2str(kk) ' x_' num2str(kk) ' n_sq_x_' num2str(kk) ' n_sq_y_' num2str(kk) ' wintx_' num2str(kk) ' winty_' num2str(kk) ' dX_' num2str(kk) ' dY_' num2str(kk)]; +end; + +eval(string_save); + +disp('done'); + +return; + +go_calib_optim; + diff --git a/src/g_calib/click_calib_fisheye_no_read.m b/src/g_calib/click_calib_fisheye_no_read.m new file mode 100755 index 0000000000000000000000000000000000000000..c773decca520a2c046ada1f1735bdc5ae698329e --- /dev/null +++ b/src/g_calib/click_calib_fisheye_no_read.m @@ -0,0 +1,260 @@ +%if exist('images_read'); +% active_images = active_images & images_read; +%end; + +var2fix = 'dX_default'; + +fixvariable; + +var2fix = 'dY_default'; + +fixvariable; + +var2fix = 'map'; + +fixvariable; + + +if ~exist('n_ima'), + data_calib_no_read; +end; + +check_active_images; + +% Step used to clean the memory if a previous atttempt has been made to read the entire set of images into memory: +for kk = 1:n_ima, + if (exist(['I_' num2str(kk)])==1), + clear(['I_' num2str(kk)]); + end; +end; + + + +fprintf(1,'\nExtraction of the grid corners on the images\n'); + + +if (exist('map')~=1), map = gray(256); end; + + +%disp('WARNING!!! Do not forget to change dX_default and dY_default in click_calib.m!!!') + +if exist('dX'), + dX_default = dX; +end; + +if exist('dY'), + dY_default = dY; +end; + +if exist('n_sq_x'), + n_sq_x_default = n_sq_x; +end; + +if exist('n_sq_y'), + n_sq_y_default = n_sq_y; +end; + + +if ~exist('dX_default')|~exist('dY_default'); + + % Setup of JY - 3D calibration rig at Intel (new at Intel) - use units in mm to match Zhang + dX_default = 30; + dY_default = 30; + + % Setup of JY - 3D calibration of the Rosette (Google) - use units in mm to match Zhang + dX_default = 0.1524; %100; + dY_default = 0.1524; %100; + +end; + + +if ~exist('n_sq_x_default')|~exist('n_sq_y_default'), + n_sq_x_default = 10; + n_sq_y_default = 10; +end; + + +if ~exist('wintx_default')|~exist('winty_default'), + if ~exist('nx'), + wintx_default = 20; + winty_default = wintx_default; + clear wintx winty + else + wintx_default = max(round(nx/128),round(ny/96)); + winty_default = wintx_default; + clear wintx winty + end; +end; + + +if ~exist('wintx') | ~exist('winty'), + clear_windows; % Clear all the window sizes (to re-initiate) +end; + + + +if ~exist('dont_ask'), + dont_ask = 0; +end; + + +if ~dont_ask, + ima_numbers = input('Number(s) of image(s) to process ([] = all images) = '); +else + ima_numbers = []; +end; + +if isempty(ima_numbers), + ima_proc = 1:n_ima; +else + ima_proc = ima_numbers; +end; + + +% Useful option to add images: +kk_first = ima_proc(1); %input('Start image number ([]=1=first): '); + +%if isempty(kk_first), kk_first = 1; end; + + +if exist(['wintx_' num2str(kk_first)]), + + eval(['wintxkk = wintx_' num2str(kk_first) ';']); + + if isempty(wintxkk) | isnan(wintxkk), + + disp('Window size for corner finder (wintx and winty):'); + wintx = input(['wintx ([] = ' num2str(wintx_default) ') = ']); + if isempty(wintx), wintx = wintx_default; end; + wintx = round(wintx); + winty = input(['winty ([] = ' num2str(winty_default) ') = ']); + if isempty(winty), winty = winty_default; end; + winty = round(winty); + + fprintf(1,'Window size = %dx%d\n',2*wintx+1,2*winty+1); + + end; + +else + + disp('Window size for corner finder (wintx and winty):'); + wintx = input(['wintx ([] = ' num2str(wintx_default) ') = ']); + if isempty(wintx), wintx = wintx_default; end; + wintx = round(wintx); + winty = input(['winty ([] = ' num2str(winty_default) ') = ']); + if isempty(winty), winty = winty_default; end; + winty = round(winty); + + fprintf(1,'Window size = %dx%d\n',2*wintx+1,2*winty+1); + +end; + +if ~dont_ask, + fprintf(1,'Do you want to use the automatic square counting mechanism (0=[]=default)\n'); + manual_squares = input(' or do you always want to enter the number of squares manually (1,other)? '); + if isempty(manual_squares), + manual_squares = 0; + else + manual_squares = ~~manual_squares; + end; +else + manual_squares = 0; +end; + +for kk = ima_proc, + + + if ~type_numbering, + number_ext = num2str(image_numbers(kk)); + else + number_ext = sprintf(['%.' num2str(N_slots) 'd'],image_numbers(kk)); + end; + + ima_name = [calib_name number_ext '.' format_image]; + + + if exist(ima_name), + + fprintf(1,'\nProcessing image %d...\n',kk); + + fprintf(1,'Loading image %s...\n',ima_name); + + if format_image(1) == 'p', + if format_image(2) == 'p', + I = double(loadppm(ima_name)); + else + I = double(loadpgm(ima_name)); + end; + else + if format_image(1) == 'r', + I = readras(ima_name); + else + I = double(imread(ima_name)); + end; + end; + + + if size(I,3)>1, + I = 0.299 * I(:,:,1) + 0.5870 * I(:,:,2) + 0.114 * I(:,:,3); + end; + + [ny,nx,junk] = size(I); + Wcal = nx; % to avoid errors later + Hcal = ny; % to avoid errors later + + click_ima_calib_fisheye_no_read; + + active_images(kk) = 1; + + else + eval(['dX_' num2str(kk) ' = NaN;']); + eval(['dY_' num2str(kk) ' = NaN;']); + + eval(['wintx_' num2str(kk) ' = NaN;']); + eval(['winty_' num2str(kk) ' = NaN;']); + + eval(['x_' num2str(kk) ' = NaN*ones(2,1);']); + eval(['X_' num2str(kk) ' = NaN*ones(3,1);']); + + eval(['n_sq_x_' num2str(kk) ' = NaN;']); + eval(['n_sq_y_' num2str(kk) ' = NaN;']); + end; +end; + + +check_active_images; + + +% Fix potential non-existing variables: + +for kk = 1:n_ima, + if ~exist(['x_' num2str(kk)]), + eval(['dX_' num2str(kk) ' = NaN;']); + eval(['dY_' num2str(kk) ' = NaN;']); + + eval(['x_' num2str(kk) ' = NaN*ones(2,1);']); + eval(['X_' num2str(kk) ' = NaN*ones(3,1);']); + + eval(['n_sq_x_' num2str(kk) ' = NaN;']); + eval(['n_sq_y_' num2str(kk) ' = NaN;']); + end; + + if ~exist(['wintx_' num2str(kk)]) | ~exist(['winty_' num2str(kk)]), + + eval(['wintx_' num2str(kk) ' = NaN;']); + eval(['winty_' num2str(kk) ' = NaN;']); + + end; +end; + + +string_save = 'save calib_data active_images ind_active wintx winty n_ima type_numbering N_slots first_num image_numbers format_image calib_name Hcal Wcal nx ny map dX_default dY_default dX dY'; + +for kk = 1:n_ima, + string_save = [string_save ' X_' num2str(kk) ' x_' num2str(kk) ' n_sq_x_' num2str(kk) ' n_sq_y_' num2str(kk) ' wintx_' num2str(kk) ' winty_' num2str(kk) ' dX_' num2str(kk) ' dY_' num2str(kk)]; +end; + +eval(string_save); + +disp('done'); + diff --git a/src/g_calib/click_calib_no_read.m b/src/g_calib/click_calib_no_read.m new file mode 100755 index 0000000000000000000000000000000000000000..aa45f0325fb5bb2d3bc344d3ca3f94a9d2bc1de0 --- /dev/null +++ b/src/g_calib/click_calib_no_read.m @@ -0,0 +1,257 @@ +%if exist('images_read'); +% active_images = active_images & images_read; +%end; + +var2fix = 'dX_default'; + +fixvariable; + +var2fix = 'dY_default'; + +fixvariable; + +var2fix = 'map'; + +fixvariable; + + +if ~exist('n_ima'), + data_calib_no_read; +end; + +check_active_images; + +% Step used to clean the memory if a previous atttempt has been made to read the entire set of images into memory: +for kk = 1:n_ima, + if (exist(['I_' num2str(kk)])==1), + clear(['I_' num2str(kk)]); + end; +end; + +fprintf(1,'\nExtraction of the grid corners on the images\n'); + + +if (exist('map')~=1), map = gray(256); end; + + +%disp('WARNING!!! Do not forget to change dX_default and dY_default in click_calib.m!!!') + +if exist('dX'), + dX_default = dX; +end; + +if exist('dY'), + dY_default = dY; +end; + +if exist('n_sq_x'), + n_sq_x_default = n_sq_x; +end; + +if exist('n_sq_y'), + n_sq_y_default = n_sq_y; +end; + + +if ~exist('dX_default')|~exist('dY_default'); + + % Setup of JY - 3D calibration rig at Intel (new at Intel) - use units in mm to match Zhang + dX_default = 30; + dY_default = 30; + + % Setup of JY - 3D calibration rig at Google - use units in mm to match Zhang + dX_default = 0.1524; %100; + dY_default = 0.1524; %100; +end; + + +if ~exist('n_sq_x_default')|~exist('n_sq_y_default'), + n_sq_x_default = 10; + n_sq_y_default = 10; +end; + + +if ~exist('wintx_default')|~exist('winty_default'), + if ~exist('nx'), + wintx_default = 30; + winty_default = wintx_default; + clear wintx winty + else + wintx_default = max(round(nx/128),round(ny/96)); + winty_default = wintx_default; + clear wintx winty + end; +end; + + +if ~exist('wintx') | ~exist('winty'), + clear_windows; % Clear all the window sizes (to re-initiate) +end; + + + +if ~exist('dont_ask'), + dont_ask = 0; +end; + + +if ~dont_ask, + ima_numbers = input('Number(s) of image(s) to process ([] = all images) = '); +else + ima_numbers = []; +end; + +if isempty(ima_numbers), + ima_proc = 1:n_ima; +else + ima_proc = ima_numbers; +end; + + +% Useful option to add images: +kk_first = ima_proc(1); %input('Start image number ([]=1=first): '); + +%if isempty(kk_first), kk_first = 1; end; + + +if exist(['wintx_' num2str(kk_first)]), + + eval(['wintxkk = wintx_' num2str(kk_first) ';']); + + if isempty(wintxkk) | isnan(wintxkk), + + disp('Window size for corner finder (wintx and winty):'); + wintx = input(['wintx ([] = ' num2str(wintx_default) ') = ']); + if isempty(wintx), wintx = wintx_default; end; + wintx = round(wintx); + winty = input(['winty ([] = ' num2str(winty_default) ') = ']); + if isempty(winty), winty = winty_default; end; + winty = round(winty); + + fprintf(1,'Window size = %dx%d\n',2*wintx+1,2*winty+1); + + end; + +else + + disp('Window size for corner finder (wintx and winty):'); + wintx = input(['wintx ([] = ' num2str(wintx_default) ') = ']); + if isempty(wintx), wintx = wintx_default; end; + wintx = round(wintx); + winty = input(['winty ([] = ' num2str(winty_default) ') = ']); + if isempty(winty), winty = winty_default; end; + winty = round(winty); + + fprintf(1,'Window size = %dx%d\n',2*wintx+1,2*winty+1); + +end; + +if ~dont_ask, + fprintf(1,'Do you want to use the automatic square counting mechanism (0=[]=default)\n'); + manual_squares = input(' or do you always want to enter the number of squares manually (1,other)? '); + if isempty(manual_squares), + manual_squares = 0; + else + manual_squares = ~~manual_squares; + end; +else + manual_squares = 0; +end; + +for kk = ima_proc, + + + if ~type_numbering, + number_ext = num2str(image_numbers(kk)); + else + number_ext = sprintf(['%.' num2str(N_slots) 'd'],image_numbers(kk)); + end; + + ima_name = [calib_name number_ext '.' format_image]; + + + if exist(ima_name), + + fprintf(1,'\nProcessing image %d...\n',kk); + + fprintf(1,'Loading image %s...\n',ima_name); + + if format_image(1) == 'p', + if format_image(2) == 'p', + I = double(loadppm(ima_name)); + else + I = double(loadpgm(ima_name)); + end; + else + if format_image(1) == 'r', + I = readras(ima_name); + else + I = double(imread(ima_name)); + end; + end; + + + if size(I,3)>1, + I = 0.299 * I(:,:,1) + 0.5870 * I(:,:,2) + 0.114 * I(:,:,3); + end; + + [ny,nx,junk] = size(I); + Wcal = nx; % to avoid errors later + Hcal = ny; % to avoid errors later + + click_ima_calib_no_read; + + active_images(kk) = 1; + + else + eval(['dX_' num2str(kk) ' = NaN;']); + eval(['dY_' num2str(kk) ' = NaN;']); + + eval(['wintx_' num2str(kk) ' = NaN;']); + eval(['winty_' num2str(kk) ' = NaN;']); + + eval(['x_' num2str(kk) ' = NaN*ones(2,1);']); + eval(['X_' num2str(kk) ' = NaN*ones(3,1);']); + + eval(['n_sq_x_' num2str(kk) ' = NaN;']); + eval(['n_sq_y_' num2str(kk) ' = NaN;']); + end; +end; + + +check_active_images; + + +% Fix potential non-existing variables: + +for kk = 1:n_ima, + if ~exist(['x_' num2str(kk)]), + eval(['dX_' num2str(kk) ' = NaN;']); + eval(['dY_' num2str(kk) ' = NaN;']); + + eval(['x_' num2str(kk) ' = NaN*ones(2,1);']); + eval(['X_' num2str(kk) ' = NaN*ones(3,1);']); + + eval(['n_sq_x_' num2str(kk) ' = NaN;']); + eval(['n_sq_y_' num2str(kk) ' = NaN;']); + end; + + if ~exist(['wintx_' num2str(kk)]) | ~exist(['winty_' num2str(kk)]), + + eval(['wintx_' num2str(kk) ' = NaN;']); + eval(['winty_' num2str(kk) ' = NaN;']); + + end; +end; + + +string_save = 'save calib_data active_images ind_active wintx winty n_ima type_numbering N_slots first_num image_numbers format_image calib_name Hcal Wcal nx ny map dX_default dY_default dX dY'; + +for kk = 1:n_ima, + string_save = [string_save ' X_' num2str(kk) ' x_' num2str(kk) ' n_sq_x_' num2str(kk) ' n_sq_y_' num2str(kk) ' wintx_' num2str(kk) ' winty_' num2str(kk) ' dX_' num2str(kk) ' dY_' num2str(kk)]; +end; + +eval(string_save); + +disp('done'); + diff --git a/src/g_calib/click_ima_calib.m b/src/g_calib/click_ima_calib.m new file mode 100755 index 0000000000000000000000000000000000000000..adb9b2ab4928b85ea920f5d5faf8a7ae85561587 --- /dev/null +++ b/src/g_calib/click_ima_calib.m @@ -0,0 +1,312 @@ +% Cleaned-up version of init_calib.m + +fprintf(1,'\nProcessing image %d...\n',kk); + +eval(['I = I_' num2str(kk) ';']); + +if exist(['wintx_' num2str(kk)]), + + eval(['wintxkk = wintx_' num2str(kk) ';']); + + if ~isempty(wintxkk) & ~isnan(wintxkk), + + eval(['wintx = wintx_' num2str(kk) ';']); + eval(['winty = winty_' num2str(kk) ';']); + + end; +end; + + +fprintf(1,'Using (wintx,winty)=(%d,%d) - Window size = %dx%d (Note: To reset the window size, run script clearwin)\n',wintx,winty,2*wintx+1,2*winty+1); +%fprintf(1,'Note: To reset the window size, clear wintx and winty and run ''Extract grid corners'' again\n'); + + +figure(2); +image(I); +colormap(map); +set(2,'color',[1 1 1]); + +title(['Click on the four extreme corners of the rectangular pattern (first corner = origin)... Image ' num2str(kk)]); + +disp('Click on the four extreme corners of the rectangular complete pattern (the first clicked corner is the origin)...'); + +x= [];y = []; +figure(2); hold on; +for count = 1:4, + [xi,yi] = ginput4(1); + [xxi] = cornerfinder([xi;yi],I,winty,wintx); + xi = xxi(1); + yi = xxi(2); + figure(2); + plot(xi,yi,'+','color',[ 1.000 0.314 0.510 ],'linewidth',2); + plot(xi + [wintx+.5 -(wintx+.5) -(wintx+.5) wintx+.5 wintx+.5],yi + [winty+.5 winty+.5 -(winty+.5) -(winty+.5) winty+.5],'-','color',[ 1.000 0.314 0.510 ],'linewidth',2); + x = [x;xi]; + y = [y;yi]; + plot(x,y,'-','color',[ 1.000 0.314 0.510 ],'linewidth',2); + drawnow; +end; +plot([x;x(1)],[y;y(1)],'-','color',[ 1.000 0.314 0.510 ],'linewidth',2); +drawnow; +hold off; + + +%[x,y] = ginput4(4); + +[Xc,good,bad,type] = cornerfinder([x';y'],I,winty,wintx); % the four corners + +x = Xc(1,:)'; +y = Xc(2,:)'; + + +% Sort the corners: +x_mean = mean(x); +y_mean = mean(y); +x_v = x - x_mean; +y_v = y - y_mean; + +theta = atan2(-y_v,x_v); +[junk,ind] = sort(theta); + +[junk,ind] = sort(mod(theta-theta(1),2*pi)); + +%ind = ind([2 3 4 1]); + +ind = ind([4 3 2 1]); %-> New: the Z axis is pointing uppward + +x = x(ind); +y = y(ind); +x1= x(1); x2 = x(2); x3 = x(3); x4 = x(4); +y1= y(1); y2 = y(2); y3 = y(3); y4 = y(4); + + +% Find center: +p_center = cross(cross([x1;y1;1],[x3;y3;1]),cross([x2;y2;1],[x4;y4;1])); +x5 = p_center(1)/p_center(3); +y5 = p_center(2)/p_center(3); + +% center on the X axis: +x6 = (x3 + x4)/2; +y6 = (y3 + y4)/2; + +% center on the Y axis: +x7 = (x1 + x4)/2; +y7 = (y1 + y4)/2; + +% Direction of displacement for the X axis: +vX = [x6-x5;y6-y5]; +vX = vX / norm(vX); + +% Direction of displacement for the X axis: +vY = [x7-x5;y7-y5]; +vY = vY / norm(vY); + +% Direction of diagonal: +vO = [x4 - x5; y4 - y5]; +vO = vO / norm(vO); + +delta = 30; + + +figure(2); +image(I); +colormap(map); +hold on; +plot([x;x(1)],[y;y(1)],'g-'); +plot(x,y,'og'); +hx=text(x6 + delta * vX(1) ,y6 + delta*vX(2),'X'); +set(hx,'color','g','Fontsize',14); +hy=text(x7 + delta*vY(1), y7 + delta*vY(2),'Y'); +set(hy,'color','g','Fontsize',14); +hO=text(x4 + delta * vO(1) ,y4 + delta*vO(2),'O','color','g','Fontsize',14); +hold off; + + +if manual_squares, + + n_sq_x = input(['Number of squares along the X direction ([]=' num2str(n_sq_x_default) ') = ']); %6 + if isempty(n_sq_x), n_sq_x = n_sq_x_default; end; + n_sq_y = input(['Number of squares along the Y direction ([]=' num2str(n_sq_y_default) ') = ']); %6 + if isempty(n_sq_y), n_sq_y = n_sq_y_default; end; + +else + + % Try to automatically count the number of squares in the grid + + n_sq_x1 = count_squares(I,x1,y1,x2,y2,wintx); + n_sq_x2 = count_squares(I,x3,y3,x4,y4,wintx); + n_sq_y1 = count_squares(I,x2,y2,x3,y3,wintx); + n_sq_y2 = count_squares(I,x4,y4,x1,y1,wintx); + + + + % If could not count the number of squares, enter manually + + if (n_sq_x1~=n_sq_x2)|(n_sq_y1~=n_sq_y2), + + + disp('Could not count the number of squares in the grid. Enter manually.'); + n_sq_x = input(['Number of squares along the X direction ([]=' num2str(n_sq_x_default) ') = ']); %6 + if isempty(n_sq_x), n_sq_x = n_sq_x_default; end; + n_sq_y = input(['Number of squares along the Y direction ([]=' num2str(n_sq_y_default) ') = ']); %6 + if isempty(n_sq_y), n_sq_y = n_sq_y_default; end; + + else + + n_sq_x = n_sq_x1; + n_sq_y = n_sq_y1; + + end; + +end; + + +n_sq_x_default = n_sq_x; +n_sq_y_default = n_sq_y; + + +if (exist('dX')~=1)|(exist('dY')~=1), % This question is now asked only once + % Enter the size of each square + + dX = input(['Size dX of each square along the X direction ([]=' num2str(dX_default) 'mm) = ']); + dY = input(['Size dY of each square along the Y direction ([]=' num2str(dY_default) 'mm) = ']); + if isempty(dX), dX = dX_default; else dX_default = dX; end; + if isempty(dY), dY = dY_default; else dY_default = dY; end; + +else + + fprintf(1,['Size of each square along the X direction: dX=' num2str(dX) 'mm\n']); + fprintf(1,['Size of each square along the Y direction: dY=' num2str(dY) 'mm (Note: To reset the size of the squares, clear the variables dX and dY)\n']); + %fprintf(1,'Note: To reset the size of the squares, clear the variables dX and dY\n'); + +end; + + +% Compute the inside points through computation of the planar homography (collineation) + +a00 = [x(1);y(1);1]; +a10 = [x(2);y(2);1]; +a11 = [x(3);y(3);1]; +a01 = [x(4);y(4);1]; + + +% Compute the planar collineation: (return the normalization matrix as well) + +[Homo,Hnorm,inv_Hnorm] = compute_homography([a00 a10 a11 a01],[0 1 1 0;0 0 1 1;1 1 1 1]); + + +% Build the grid using the planar collineation: + +x_l = ((0:n_sq_x)'*ones(1,n_sq_y+1))/n_sq_x; +y_l = (ones(n_sq_x+1,1)*(0:n_sq_y))/n_sq_y; +pts = [x_l(:) y_l(:) ones((n_sq_x+1)*(n_sq_y+1),1)]'; + +XX = Homo*pts; +XX = XX(1:2,:) ./ (ones(2,1)*XX(3,:)); + + +% Complete size of the rectangle + +W = n_sq_x*dX; +L = n_sq_y*dY; + + + + +%%%%%%%%%%%%%%%%%%%%%%%% ADDITIONAL STUFF IN THE CASE OF HIGHLY DISTORTED IMAGES %%%%%%%%%%%%% +figure(2); +hold on; +plot(XX(1,:),XX(2,:),'r+'); +title('The red crosses should be close to the image corners'); +hold off; + +disp('If the guessed grid corners (red crosses on the image) are not close to the actual corners,'); +disp('it is necessary to enter an initial guess for the radial distortion factor kc (useful for subpixel detection)'); +quest_distort = input('Need of an initial guess for distortion? ([]=no, other=yes) '); + +quest_distort = ~isempty(quest_distort); + +if quest_distort, + % Estimation of focal length: + c_g = [size(I,2);size(I,1)]/2 + .5; + f_g = Distor2Calib(0,[[x(1) x(2) x(4) x(3)] - c_g(1);[y(1) y(2) y(4) y(3)] - c_g(2)],1,1,4,W,L,[-W/2 W/2 W/2 -W/2;L/2 L/2 -L/2 -L/2; 0 0 0 0],100,1,1); + f_g = mean(f_g); + script_fit_distortion; +end; +%%%%%%%%%%%%%%%%%%%%% END ADDITIONAL STUFF IN THE CASE OF HIGHLY DISTORTED IMAGES %%%%%%%%%%%%% + + + + + +Np = (n_sq_x+1)*(n_sq_y+1); + +disp('Corner extraction...'); + +grid_pts = cornerfinder(XX,I,winty,wintx); %%% Finds the exact corners at every points! + + + +%save all_corners x y grid_pts + +grid_pts = grid_pts - 1; % subtract 1 to bring the origin to (0,0) instead of (1,1) in matlab (not necessary in C) + + + +ind_corners = [1 n_sq_x+1 (n_sq_x+1)*n_sq_y+1 (n_sq_x+1)*(n_sq_y+1)]; % index of the 4 corners +ind_orig = (n_sq_x+1)*n_sq_y + 1; +xorig = grid_pts(1,ind_orig); +yorig = grid_pts(2,ind_orig); +dxpos = mean([grid_pts(:,ind_orig) grid_pts(:,ind_orig+1)]'); +dypos = mean([grid_pts(:,ind_orig) grid_pts(:,ind_orig-n_sq_x-1)]'); + + +x_box_kk = [grid_pts(1,:)-(wintx+.5);grid_pts(1,:)+(wintx+.5);grid_pts(1,:)+(wintx+.5);grid_pts(1,:)-(wintx+.5);grid_pts(1,:)-(wintx+.5)]; +y_box_kk = [grid_pts(2,:)-(winty+.5);grid_pts(2,:)-(winty+.5);grid_pts(2,:)+(winty+.5);grid_pts(2,:)+(winty+.5);grid_pts(2,:)-(winty+.5)]; + + +figure(3); +image(I); colormap(map); hold on; +plot(grid_pts(1,:)+1,grid_pts(2,:)+1,'r+'); +plot(x_box_kk+1,y_box_kk+1,'-b'); +plot(grid_pts(1,ind_corners)+1,grid_pts(2,ind_corners)+1,'mo'); +plot(xorig+1,yorig+1,'*m'); +h = text(xorig+delta*vO(1),yorig+delta*vO(2),'O'); +set(h,'Color','m','FontSize',14); +h2 = text(dxpos(1)+delta*vX(1),dxpos(2)+delta*vX(2),'dX'); +set(h2,'Color','g','FontSize',14); +h3 = text(dypos(1)+delta*vY(1),dypos(2)+delta*vY(2),'dY'); +set(h3,'Color','g','FontSize',14); +xlabel('Xc (in camera frame)'); +ylabel('Yc (in camera frame)'); +title('Extracted corners'); +zoom on; +drawnow; +hold off; + + +Xi = reshape(([0:n_sq_x]*dX)'*ones(1,n_sq_y+1),Np,1)'; +Yi = reshape(ones(n_sq_x+1,1)*[n_sq_y:-1:0]*dY,Np,1)'; +Zi = zeros(1,Np); + +Xgrid = [Xi;Yi;Zi]; + + +% All the point coordinates (on the image, and in 3D) - for global optimization: + +x = grid_pts; +X = Xgrid; + + +% Saves all the data into variables: + +eval(['dX_' num2str(kk) ' = dX;']); +eval(['dY_' num2str(kk) ' = dY;']); + +eval(['wintx_' num2str(kk) ' = wintx;']); +eval(['winty_' num2str(kk) ' = winty;']); + +eval(['x_' num2str(kk) ' = x;']); +eval(['X_' num2str(kk) ' = X;']); + +eval(['n_sq_x_' num2str(kk) ' = n_sq_x;']); +eval(['n_sq_y_' num2str(kk) ' = n_sq_y;']); diff --git a/src/g_calib/click_ima_calib3D.m b/src/g_calib/click_ima_calib3D.m new file mode 100755 index 0000000000000000000000000000000000000000..0109ccab1433587bec9a3d294fec071295fa2d88 --- /dev/null +++ b/src/g_calib/click_ima_calib3D.m @@ -0,0 +1,482 @@ + % Cleaned-up version of init_calib.m + + eval(['I = I_' num2str(kk) ';']); + + figure(2); + image(I); + colormap(map); + + + + + + %%%%%%%%%%%%%%%%%%%%%%%%% LEFT PATTERN ACQUISITION %%%%%%%%%%%%%%%%%%%%%%%%%%%%% + + + + title(['Click on the four extreme corners of the left rectangular pattern... Image ' num2str(kk)]); + + disp('Click on the four extreme corners of the left rectangular pattern...'); + + [x,y] = ginput4(4); + + [Xc,good,bad,type] = cornerfinder([x';y'],I,winty,wintx); % the four corners + + x = Xc(1,:)'; + y = Xc(2,:)'; + + [y,indy] = sort(y); + x = x(indy); + + if (x(2) > x(1)), + x4 = x(1);y4 = y(1); x3 = x(2); y3 = y(2); + else + x4 = x(2);y4 = y(2); x3 = x(1); y3 = y(1); + end; + if (x(3) > x(4)), + x2 = x(3);y2 = y(3); x1 = x(4); y1 = y(4); + else + x2 = x(4);y2 = y(4); x1 = x(3); y1 = y(3); + end; + + x = [x1;x2;x3;x4]; + y = [y1;y2;y3;y4]; + + + figure(2); hold on; + plot([x;x(1)],[y;y(1)],'g-'); + plot(x,y,'og'); + hx=text((x(4)+x(3))/2,(y(4)+y(3))/2 - 20,'X'); + set(hx,'color','g','Fontsize',14); + hy=text((x(4)+x(1))/2-20,(y(4)+y(1))/2,'Y'); + set(hy,'color','g','Fontsize',14); + hold off; + + drawnow; + + + % Try to automatically count the number of squares in the grid + + n_sq_x1 = count_squares(I,x1,y1,x2,y2,wintx); + n_sq_x2 = count_squares(I,x3,y3,x4,y4,wintx); + n_sq_y1 = count_squares(I,x2,y2,x3,y3,wintx); + n_sq_y2 = count_squares(I,x4,y4,x1,y1,wintx); + + + + % If could not count the number of squares, enter manually + + if (n_sq_x1~=n_sq_x2)|(n_sq_y1~=n_sq_y2), + + + disp('Could not count the number of squares in the grid. Enter manually.'); + n_sq_x = input('Number of squares along the X direction ([]=10) = '); %6 + if isempty(n_sq_x), n_sq_x = 10; end; + n_sq_y = input('Number of squares along the Y direction ([]=10) = '); %6 + if isempty(n_sq_y), n_sq_y = 10; end; + + else + + n_sq_x = n_sq_x1; + n_sq_y = n_sq_y1; + + end; + + + if 1, + % Enter the size of each square + + dX = input(['Size dX of each square along the X direction ([]=' num2str(dX_default) 'cm) = ']); + dY = input(['Size dY of each square along the Y direction ([]=' num2str(dY_default) 'cm) = ']); + if isempty(dX), dX = dX_default; else dX_default = dX; end; + if isempty(dY), dY = dY_default; else dY_default = dY; end; + + else + + dX = 3; + dY = 3; + + end; + + + % Compute the inside points through computation of the planar homography (collineation) + + a00 = [x(1);y(1);1]; + a10 = [x(2);y(2);1]; + a11 = [x(3);y(3);1]; + a01 = [x(4);y(4);1]; + + + % Compute the planart collineation: (return the normalization matrice as well) + + [Homo,Hnorm,inv_Hnorm] = compute_collineation (a00, a10, a11, a01); + + + % Build the grid using the planar collineation: + + x_l = ((0:n_sq_x)'*ones(1,n_sq_y+1))/n_sq_x; + y_l = (ones(n_sq_x+1,1)*(0:n_sq_y))/n_sq_y; + pts = [x_l(:) y_l(:) ones((n_sq_x+1)*(n_sq_y+1),1)]'; + + XX = Homo*pts; + XX = XX(1:2,:) ./ (ones(2,1)*XX(3,:)); + + + % Complete size of the rectangle + + W = n_sq_x*dX; + L = n_sq_y*dY; + + + + if 1, + %%%%%%%%%%%%%%%%%%%%%%%% ADDITIONAL STUFF IN THE CASE OF HIGHLY DISTORTED IMAGES %%%%%%%%%%%%% + figure(2); + hold on; + plot(XX(1,:),XX(2,:),'r+'); + title('The red crosses should be close to the image corners'); + hold off; + + disp('If the guessed grid corners (red crosses on the image) are not close to the actual corners,'); + disp('it is necessary to enter an initial guess for the radial distortion factor kc (useful for subpixel detection)'); + quest_distort = input('Need of an initial guess for distortion? ([]=no, other=yes) '); + + quest_distort = ~isempty(quest_distort); + + if quest_distort, + % Estimation of focal length: + c_g = [size(I,2);size(I,1)]/2 + .5; + f_g = Distor2Calib(0,[[x(1) x(2) x(4) x(3)] - c_g(1);[y(1) y(2) y(4) y(3)] - c_g(2)],1,1,4,W,L,[-W/2 W/2 W/2 -W/2;L/2 L/2 -L/2 -L/2; 0 0 0 0],100,1,1); + f_g = mean(f_g); + script_fit_distortion; + end; + %%%%%%%%%%%%%%%%%%%%% END ADDITIONAL STUFF IN THE CASE OF HIGHLY DISTORTED IMAGES %%%%%%%%%%%%% + end; + + + Np = (n_sq_x+1)*(n_sq_y+1); + + disp('Corner extraction...'); + + grid_pts = cornerfinder(XX,I,winty,wintx); %%% Finds the exact corners at every points! + + %save all_corners x y grid_pts + + grid_pts = grid_pts - 1; % subtract 1 to bring the origin to (0,0) instead of (1,1) in matlab (not necessary in C) + + + % Global Homography from plane to pixel coordinates: + + H_total = [1 0 -1 ; 0 1 -1 ; 0 0 1]*Homo*[1 0 0;0 -1 1;0 0 1]*[1/W 0 0 ; 0 1/L 0; 0 0 1]; + % WARNING!!! the first matrix (on the left side) takes care of the transformation of the pixel cooredinates by -1 (previous line) + % If it is not done, then this matrix should not appear (in C) + H_total = H_total / H_total(3,3); + + + ind_corners = [1 n_sq_x+1 (n_sq_x+1)*n_sq_y+1 (n_sq_x+1)*(n_sq_y+1)]; % index of the 4 corners + ind_orig = (n_sq_x+1)*n_sq_y + 1; + xorig = grid_pts(1,ind_orig); + yorig = grid_pts(2,ind_orig); + dxpos = mean([grid_pts(:,ind_orig) grid_pts(:,ind_orig+1)]'); + dypos = mean([grid_pts(:,ind_orig) grid_pts(:,ind_orig-n_sq_x-1)]'); + + + x_box_kk = [grid_pts(1,:)-(wintx+.5);grid_pts(1,:)+(wintx+.5);grid_pts(1,:)+(wintx+.5);grid_pts(1,:)-(wintx+.5);grid_pts(1,:)-(wintx+.5)]; + y_box_kk = [grid_pts(2,:)-(winty+.5);grid_pts(2,:)-(winty+.5);grid_pts(2,:)+(winty+.5);grid_pts(2,:)+(winty+.5);grid_pts(2,:)-(winty+.5)]; + + + figure(3); + image(I); colormap(map); hold on; + plot(grid_pts(1,:)+1,grid_pts(2,:)+1,'r+'); + plot(x_box_kk+1,y_box_kk+1,'-b'); + plot(grid_pts(1,ind_corners)+1,grid_pts(2,ind_corners)+1,'mo'); + plot(xorig+1,yorig+1,'*m'); + h = text(xorig-15,yorig-15,'O'); + set(h,'Color','m','FontSize',14); + h2 = text(dxpos(1)-10,dxpos(2)-10,'dX'); + set(h2,'Color','g','FontSize',14); + h3 = text(dypos(1)-25,dypos(2)-3,'dY'); + set(h3,'Color','g','FontSize',14); + xlabel('Xc (in camera frame)'); + ylabel('Yc (in camera frame)'); + title('Extracted corners'); + zoom on; + drawnow; + hold off; + + + Xi = reshape(([0:n_sq_x]*dX)'*ones(1,n_sq_y+1),Np,1)'; + Yi = reshape(ones(n_sq_x+1,1)*[n_sq_y:-1:0]*dY,Np,1)'; + Zi = zeros(1,Np); + + Xgrid = [Xi;Yi;Zi]; + + + % All the point coordinates (on the image, and in 3D) - for global optimization: + + x = grid_pts; + X = Xgrid; + + + % The left pannel info: + + xl = x; + Xl = X; + nl_sq_x = n_sq_x; + nl_sq_y = n_sq_y; + Hl = H_total; + + + + + + + %%%%%%%%%%%%%%%%%%%%%%%%% RIGHT PATTERN ACQUISITION %%%%%%%%%%%%%%%%%%%%%%%%%%%%% + + + x1 = a10(1)/a10(3); + x4 = a11(1)/a11(3); + + y1 = a10(2)/a10(3); + y4 = a11(2)/a11(3); + + + figure(2); + hold on; + plot([x1 x4],[y1 y4],'c-'); + plot([x1 x4],[y1 y4],'co'); + hold off; + + title(['Click on the two remaining extreme corners of the right rectangular pattern... Image ' num2str(kk)]); + + disp('Click on the two remaining extreme corners of the right rectangular pattern...'); + + [x,y] = ginput4(2); + + [Xc,good,bad,type] = cornerfinder([x';y'],I,winty,wintx); % the four corners + + x = Xc(1,:)'; + y = Xc(2,:)'; + + [y,indy] = sort(y); + x = x(indy); + + x2 = x(2); + x3 = x(1); + + y2 = y(2); + y3 = y(1); + + + x = [x1;x2;x3;x4]; + y = [y1;y2;y3;y4]; + + figure(2); hold on; + plot([x;x(1)],[y;y(1)],'c-'); + plot(x,y,'oc'); + hx=text((x(4)+x(3))/2,(y(4)+y(3))/2 - 20,'X'); + set(hx,'color','c','Fontsize',14); + hy=text((x(4)+x(1))/2-20,(y(4)+y(1))/2,'Y'); + set(hy,'color','c','Fontsize',14); + hold off; + drawnow; + + + % Try to automatically count the number of squares in the grid + + n_sq_x1 = count_squares(I,x1,y1,x2,y2,wintx); + n_sq_x2 = count_squares(I,x3,y3,x4,y4,wintx); + n_sq_y1 = count_squares(I,x2,y2,x3,y3,wintx); + n_sq_y2 = count_squares(I,x4,y4,x1,y1,wintx); + + + + % If could not count the number of squares, enter manually + + if (n_sq_x1~=n_sq_x2)|(n_sq_y1~=n_sq_y2), + + + disp('Could not count the number of squares in the grid. Enter manually.'); + n_sq_x = input('Number of squares along the X direction ([]=10) = '); %6 + if isempty(n_sq_x), n_sq_x = 10; end; + n_sq_y = input('Number of squares along the Y direction ([]=10) = '); %6 + if isempty(n_sq_y), n_sq_y = 10; end; + + else + + n_sq_x = n_sq_x1; + n_sq_y = n_sq_y1; + + end; + + + if 1, + % Enter the size of each square + + dX = input(['Size dX of each square along the X direction ([]=' num2str(dX_default) 'cm) = ']); + dY = input(['Size dY of each square along the Y direction ([]=' num2str(dY_default) 'cm) = ']); + if isempty(dX), dX = dX_default; else dX_default = dX; end; + if isempty(dY), dY = dY_default; else dY_default = dY; end; + + else + + dX = 3; + dY = 3; + + end; + + + % Compute the inside points through computation of the planar homography (collineation) + + a00 = [x(1);y(1);1]; + a10 = [x(2);y(2);1]; + a11 = [x(3);y(3);1]; + a01 = [x(4);y(4);1]; + + + % Compute the planart collineation: (return the normalization matrice as well) + + [Homo,Hnorm,inv_Hnorm] = compute_collineation (a00, a10, a11, a01); + + + % Build the grid using the planar collineation: + + x_l = ((0:n_sq_x)'*ones(1,n_sq_y+1))/n_sq_x; + y_l = (ones(n_sq_x+1,1)*(0:n_sq_y))/n_sq_y; + pts = [x_l(:) y_l(:) ones((n_sq_x+1)*(n_sq_y+1),1)]'; + + XX = Homo*pts; + XX = XX(1:2,:) ./ (ones(2,1)*XX(3,:)); + + + % Complete size of the rectangle + + W = n_sq_x*dX; + L = n_sq_y*dY; + + + + if 1, + %%%%%%%%%%%%%%%%%%%%%%%% ADDITIONAL STUFF IN THE CASE OF HIGHLY DISTORTED IMAGES %%%%%%%%%%%%% + figure(2); + hold on; + plot(XX(1,:),XX(2,:),'r+'); + title('The red crosses should be close to the image corners'); + hold off; + + disp('If the guessed grid corners (red crosses on the image) are not close to the actual corners,'); + disp('it is necessary to enter an initial guess for the radial distortion factor kc (useful for subpixel detection)'); + quest_distort = input('Need of an initial guess for distortion? ([]=no, other=yes) '); + + quest_distort = ~isempty(quest_distort); + + if quest_distort, + % Estimation of focal length: + c_g = [size(I,2);size(I,1)]/2 + .5; + f_g = Distor2Calib(0,[[x(1) x(2) x(4) x(3)] - c_g(1);[y(1) y(2) y(4) y(3)] - c_g(2)],1,1,4,W,L,[-W/2 W/2 W/2 -W/2;L/2 L/2 -L/2 -L/2; 0 0 0 0],100,1,1); + f_g = mean(f_g); + script_fit_distortion; + end; + %%%%%%%%%%%%%%%%%%%%% END ADDITIONAL STUFF IN THE CASE OF HIGHLY DISTORTED IMAGES %%%%%%%%%%%%% + end; + + + Np = (n_sq_x+1)*(n_sq_y+1); + + disp('Corner extraction...'); + + grid_pts = cornerfinder(XX,I,winty,wintx); %%% Finds the exact corners at every points! + + %save all_corners x y grid_pts + + grid_pts = grid_pts - 1; % subtract 1 to bring the origin to (0,0) instead of (1,1) in matlab (not necessary in C) + + + % Global Homography from plane to pixel coordinates: + + H_total = [1 0 -1 ; 0 1 -1 ; 0 0 1]*Homo*[1 0 0;0 -1 1;0 0 1]*[1/W 0 0 ; 0 1/L 0; 0 0 1]; + % WARNING!!! the first matrix (on the left side) takes care of the transformation of the pixel cooredinates by -1 (previous line) + % If it is not done, then this matrix should not appear (in C) + H_total = H_total / H_total(3,3); + + + ind_corners = [1 n_sq_x+1 (n_sq_x+1)*n_sq_y+1 (n_sq_x+1)*(n_sq_y+1)]; % index of the 4 corners + ind_orig = (n_sq_x+1)*n_sq_y + 1; + xorig = grid_pts(1,ind_orig); + yorig = grid_pts(2,ind_orig); + dxpos = mean([grid_pts(:,ind_orig) grid_pts(:,ind_orig+1)]'); + dypos = mean([grid_pts(:,ind_orig) grid_pts(:,ind_orig-n_sq_x-1)]'); + + + x_box_kk = [grid_pts(1,:)-(wintx+.5);grid_pts(1,:)+(wintx+.5);grid_pts(1,:)+(wintx+.5);grid_pts(1,:)-(wintx+.5);grid_pts(1,:)-(wintx+.5)]; + y_box_kk = [grid_pts(2,:)-(winty+.5);grid_pts(2,:)-(winty+.5);grid_pts(2,:)+(winty+.5);grid_pts(2,:)+(winty+.5);grid_pts(2,:)-(winty+.5)]; + + + figure(3); + hold on; + plot(grid_pts(1,:)+1,grid_pts(2,:)+1,'r+'); + plot(x_box_kk+1,y_box_kk+1,'-b'); + plot(grid_pts(1,ind_corners)+1,grid_pts(2,ind_corners)+1,'mo'); + plot(xorig+1,yorig+1,'*m'); + h = text(xorig-15,yorig-15,'O'); + set(h,'Color','m','FontSize',14); + h2 = text(dxpos(1)-10,dxpos(2)-10,'dX'); + set(h2,'Color','g','FontSize',14); + h3 = text(dypos(1)-25,dypos(2)-3,'dY'); + set(h3,'Color','g','FontSize',14); + xlabel('Xc (in camera frame)'); + ylabel('Yc (in camera frame)'); + title('Extracted corners'); + zoom on; + drawnow; + hold off; + + + Xi = reshape(([0:n_sq_x]*dX)'*ones(1,n_sq_y+1),Np,1)'; + Yi = reshape(ones(n_sq_x+1,1)*[n_sq_y:-1:0]*dY,Np,1)'; + Zi = zeros(1,Np); + + Xgrid = [Xi;Yi;Zi]; + + + % All the point coordinates (on the image, and in 3D) - for global optimization: + + x = grid_pts; + X = Xgrid; + + + % The right pannel info: + + xr = x; + Xr = X; + nr_sq_x = n_sq_x; + nr_sq_y = n_sq_y; + Hr = H_total; + + + +%%%%%%%% REGROUP THE LEFT AND RIHT PATTERNS %%%%%%%%%%%%% + + +Xr2 = [0 0 1;0 1 0;-1 0 0]*Xr + [dX*nl_sq_x;0;0]*ones(1,length(Xr)); + + +x = [xl xr]; + +X = [Xl Xr2]; + + + + eval(['x_' num2str(kk) ' = x;']); + eval(['X_' num2str(kk) ' = X;']); + + eval(['nl_sq_x_' num2str(kk) ' = nl_sq_x;']); + eval(['nl_sq_y_' num2str(kk) ' = nl_sq_y;']); + + eval(['nr_sq_x_' num2str(kk) ' = nr_sq_x;']); + eval(['nr_sq_y_' num2str(kk) ' = nr_sq_y;']); + + % Save the global planar homography: + + eval(['Hl_' num2str(kk) ' = Hl;']); + eval(['Hr_' num2str(kk) ' = Hr;']); \ No newline at end of file diff --git a/src/g_calib/click_ima_calib_fisheye_no_read.m b/src/g_calib/click_ima_calib_fisheye_no_read.m new file mode 100755 index 0000000000000000000000000000000000000000..ceb51eac7aa4d6c637ea43f33adf991970b0e065 --- /dev/null +++ b/src/g_calib/click_ima_calib_fisheye_no_read.m @@ -0,0 +1,308 @@ +% Rough estimates for principal point and focal length: +fg = [1009.661057548567 1009.706970843858 ]'; +cg = [ 1031.688786545889 1288.395050759215]'; +kg = [-0.054285891395760 -0.026812583950935 0 0]'; + +%fg = [ 139.77712 139.61650 ]'; +%cg = [ 319.62550 258.16616 ]'; +%kg = [ 0.02859 -0.01812 0.00839 -0.00182 ]'; + +%fg = [700;700]; %[ 1397.7712 1396.1650 ]'; +%cg = [ 319.62550 258.16616 ]'; +%kg = [ -1 -0.01812 0.00839 -0.00182 ]'; + +if exist(['wintx_' num2str(kk)]), + eval(['wintxkk = wintx_' num2str(kk) ';']); + if ~isempty(wintxkk) & ~isnan(wintxkk), + eval(['wintx = wintx_' num2str(kk) ';']); + eval(['winty = winty_' num2str(kk) ';']); + end; +end; + +if ~exist('rosette_calibration', 'var') + rosette_calibration = 0; +end; + +if (rosette_calibration), + wintx = 20; + winty = 20; +end; + +fprintf(1,'Using (wintx,winty)=(%d,%d) - Window size = %dx%d (Note: To reset the window size, run script clearwin)\n',wintx,winty,2*wintx+1,2*winty+1); + +grid_success = 0; + +while (~grid_success) + + figure(2); clf; + image(I); + axis image; + colormap(map); + set(2,'color',[1 1 1]); + title(['Click on the four extreme corners of the rectangular pattern (first corner = origin)... Image ' num2str(kk)]); + disp('Click on the four extreme corners of the rectangular complete pattern (the first clicked corner is the origin)...'); + + x= [];y = []; + figure(2); hold on; + for count = 1:4, + [xi,yi] = ginput4(1); + [xxi] = cornerfinder([xi;yi],I,winty,wintx); + xi = xxi(1); + yi = xxi(2); + figure(2); + plot(xi,yi,'+','color',[ 1.000 0.314 0.510 ],'linewidth',2); + plot(xi + [wintx+.5 -(wintx+.5) -(wintx+.5) wintx+.5 wintx+.5],yi + [winty+.5 winty+.5 -(winty+.5) -(winty+.5) winty+.5],'-','color',[ 1.000 0.314 0.510 ],'linewidth',2); + x = [x;xi]; + y = [y;yi]; + plot(x,y,'-','color',[ 1.000 0.314 0.510 ],'linewidth',2); + drawnow; + end; + plot([x;x(1)],[y;y(1)],'-','color',[ 1.000 0.314 0.510 ],'linewidth',2); + drawnow; + hold off; + + [Xc,good,bad,type] = cornerfinder([x';y'],I,winty,wintx); % the four corners + + x = Xc(1,:)'; + y = Xc(2,:)'; + + % Sort the corners: + x_mean = mean(x); + y_mean = mean(y); + x_v = x - x_mean; + y_v = y - y_mean; + + theta = atan2(-y_v,x_v); + [junk,ind] = sort(theta); + + [junk,ind] = sort(mod(theta-theta(1),2*pi)); + + ind = ind([4 3 2 1]); %-> New: the Z axis is pointing uppward + + x = x(ind); + y = y(ind); + + if (rosette_calibration && (kk < 17)) + % Enforce the ordering convention for the Rosette calibration. + c_ima = [nx/2 + 0.5 ; ny/2 + 0.5]; + + vects = [x'-c_ima(1) ; y'-c_ima(2)]; + r = sqrt(sum(vects.^2)); + + [r_junk,ind_sort_r] = sort(r); + ind_23 = ind_sort_r(1:2); + ind_14 = ind_sort_r(3:4); + + if det(vects(:,ind_23))<0 + ind2 = ind_23(1); + ind3 = ind_23(2); + else + ind2 = ind_23(2); + ind3 = ind_23(1); + end; + if det(vects(:,ind_14))<0 + ind1 = ind_14(1); + ind4 = ind_14(2); + else + ind1 = ind_14(2); + ind4 = ind_14(1); + end; + + ind_resort = [ind1;ind2;ind3;ind4]; + x = x(ind_resort); + y = y(ind_resort); + end; + + x1= x(1); x2 = x(2); x3 = x(3); x4 = x(4); + y1= y(1); y2 = y(2); y3 = y(3); y4 = y(4); + + % Find center: + p_center = cross(cross([x1;y1;1],[x3;y3;1]),cross([x2;y2;1],[x4;y4;1])); + x5 = p_center(1)/p_center(3); + y5 = p_center(2)/p_center(3); + + % center on the X axis: + x6 = (x3 + x4)/2; + y6 = (y3 + y4)/2; + + % center on the Y axis: + x7 = (x1 + x4)/2; + y7 = (y1 + y4)/2; + + % Direction of displacement for the X axis: + vX = [x6-x5;y6-y5]; + vX = vX / norm(vX); + + % Direction of displacement for the X axis: + vY = [x7-x5;y7-y5]; + vY = vY / norm(vY); + + % Direction of diagonal: + vO = [x4 - x5; y4 - y5]; + vO = vO / norm(vO); + + delta = 30; + + figure(2); image(I); + axis image; + colormap(map); + hold on; + plot([x;x(1)],[y;y(1)],'g-'); + plot(x,y,'og'); + hx=text(x6 + delta * vX(1) ,y6 + delta*vX(2),'X'); + set(hx,'color','g','Fontsize',14); + hy=text(x7 + delta*vY(1), y7 + delta*vY(2),'Y'); + set(hy,'color','g','Fontsize',14); + hO=text(x4 + delta * vO(1) ,y4 + delta*vO(2),'O','color','g','Fontsize',14); + for iii = 1:4, + text(x(iii),y(iii),num2str(iii)); + end; + hold off; + + if manual_squares, + n_sq_x = input(['Number of squares along the X direction ([]=' num2str(n_sq_x_default) ') = ']); %6 + if isempty(n_sq_x), n_sq_x = n_sq_x_default; end; + n_sq_y = input(['Number of squares along the Y direction ([]=' num2str(n_sq_y_default) ') = ']); %6 + if isempty(n_sq_y), n_sq_y = n_sq_y_default; end; + grid_success = 1; + else + % Try to automatically count the number of squares in the grid + if (rosette_calibration) + win_count = 10; + else + win_count = wintx; + end; + n_sq_x1 = count_squares_fisheye_distorted(I,x1,y1,x2,y2,win_count, fg, cg, kg); + n_sq_x2 = count_squares_fisheye_distorted(I,x3,y3,x4,y4,win_count, fg, cg, kg); + n_sq_y1 = count_squares_fisheye_distorted(I,x2,y2,x3,y3,win_count, fg, cg, kg); + n_sq_y2 = count_squares_fisheye_distorted(I,x4,y4,x1,y1,win_count, fg, cg, kg); + %n_sq_x1 = count_squares(I,x1,y1,x2,y2,wintx); + %n_sq_x2 = count_squares(I,x3,y3,x4,y4,wintx); + %n_sq_y1 = count_squares(I,x2,y2,x3,y3,wintx); + %n_sq_y2 = count_squares(I,x4,y4,x1,y1,wintx); + + % If could not count the number of squares, enter manually + if (n_sq_x1~=n_sq_x2)|(n_sq_y1~=n_sq_y2), + if ~rosette_calibration, + disp('Could not count the number of squares in the grid. Enter manually.'); + n_sq_x = input(['Number of squares along the X direction ([]=' num2str(n_sq_x_default) ') = ']); %6 + if isempty(n_sq_x), n_sq_x = n_sq_x_default; end; + n_sq_y = input(['Number of squares along the Y direction ([]=' num2str(n_sq_y_default) ') = ']); %6 + if isempty(n_sq_y), n_sq_y = n_sq_y_default; end; + grid_success = 1; + else + grid_success = 0; + end; + else + n_sq_x = n_sq_x1; + n_sq_y = n_sq_y1; + grid_success = 1; + end; + end; + + if ~grid_success + fprintf(1,'Invalid grid. Try again.\n'); + end; + +end; + +n_sq_x_default = n_sq_x; +n_sq_y_default = n_sq_y; + +if (exist('dX')~=1)|(exist('dY')~=1), % This question is now asked only once + % Enter the size of each square + dX = input(['Size dX of each square along the X direction ([]=' num2str(dX_default) 'mm) = ']); + dY = input(['Size dY of each square along the Y direction ([]=' num2str(dY_default) 'mm) = ']); + if isempty(dX), dX = dX_default; else dX_default = dX; end; + if isempty(dY), dY = dY_default; else dY_default = dY; end; +else + fprintf(1,['Size of each square along the X direction: dX=' num2str(dX) 'mm\n']); + fprintf(1,['Size of each square along the Y direction: dY=' num2str(dY) 'mm (Note: To reset the size of the squares, clear the variables dX and dY)\n']); +end; + +x_n = (x - 1 - cg(1))/fg(1); +y_n = (y - 1 - cg(2))/fg(2); + +[x_pn] = comp_fisheye_distortion([x_n' ; y_n'],kg); + +% Compute the inside points through computation of the planar homography (collineation) +a00 = [x_pn(1,1);x_pn(2,1);1]; +a10 = [x_pn(1,2);x_pn(2,2);1]; +a11 = [x_pn(1,3);x_pn(2,3);1]; +a01 = [x_pn(1,4);x_pn(2,4);1]; + +% Compute the planar collineation: (return the normalization matrix as well) +[Homo,Hnorm,inv_Hnorm] = compute_homography([a00 a10 a11 a01],[0 1 1 0;0 0 1 1;1 1 1 1]); + +% Build the grid using the planar collineation: +x_l = ((0:n_sq_x)'*ones(1,n_sq_y+1))/n_sq_x; +y_l = (ones(n_sq_x+1,1)*(0:n_sq_y))/n_sq_y; +pts = [x_l(:) y_l(:) ones((n_sq_x+1)*(n_sq_y+1),1)]'; + +XXpn = Homo*pts; +XXpn = XXpn(1:2,:) ./ (ones(2,1)*XXpn(3,:)); + +XX = apply_fisheye_distortion(XXpn,kg); + +XX(1,:) = fg(1)*XX(1,:) + cg(1) + 1; +XX(2,:) = fg(2)*XX(2,:) + cg(2) + 1; + +% Complete size of the rectangle +W = n_sq_x*dX; +L = n_sq_y*dY; + +Np = (n_sq_x+1)*(n_sq_y+1); +disp('Corner extraction...'); +grid_pts = cornerfinder(XX,I,winty,wintx); %%% Finds the exact corners at every points! +%grid_pts = XX; %%% Finds the exact corners at every points! + +grid_pts = grid_pts - 1; % subtract 1 to bring the origin to (0,0) instead of (1,1) in matlab (not necessary in C) + +ind_corners = [1 n_sq_x+1 (n_sq_x+1)*n_sq_y+1 (n_sq_x+1)*(n_sq_y+1)]; % index of the 4 corners +ind_orig = (n_sq_x+1)*n_sq_y + 1; +xorig = grid_pts(1,ind_orig); +yorig = grid_pts(2,ind_orig); +dxpos = mean([grid_pts(:,ind_orig) grid_pts(:,ind_orig+1)]'); +dypos = mean([grid_pts(:,ind_orig) grid_pts(:,ind_orig-n_sq_x-1)]'); + +x_box_kk = [grid_pts(1,:)-(wintx+.5);grid_pts(1,:)+(wintx+.5);grid_pts(1,:)+(wintx+.5);grid_pts(1,:)-(wintx+.5);grid_pts(1,:)-(wintx+.5)]; +y_box_kk = [grid_pts(2,:)-(winty+.5);grid_pts(2,:)-(winty+.5);grid_pts(2,:)+(winty+.5);grid_pts(2,:)+(winty+.5);grid_pts(2,:)-(winty+.5)]; + +figure(3); +image(I); axis image; colormap(map); hold on; +plot(grid_pts(1,:)+1,grid_pts(2,:)+1,'r+'); +plot(x_box_kk+1,y_box_kk+1,'-b'); +plot(grid_pts(1,ind_corners)+1,grid_pts(2,ind_corners)+1,'mo'); +plot(xorig+1,yorig+1,'*m'); +h = text(xorig+delta*vO(1),yorig+delta*vO(2),'O'); +set(h,'Color','m','FontSize',14); +h2 = text(dxpos(1)+delta*vX(1),dxpos(2)+delta*vX(2),'dX'); +set(h2,'Color','g','FontSize',14); +h3 = text(dypos(1)+delta*vY(1),dypos(2)+delta*vY(2),'dY'); +set(h3,'Color','g','FontSize',14); +xlabel('Xc (in camera frame)'); +ylabel('Yc (in camera frame)'); +title('Extracted corners'); +zoom on; +drawnow; +hold off; + +Xi = reshape(([0:n_sq_x]*dX)'*ones(1,n_sq_y+1),Np,1)'; +Yi = reshape(ones(n_sq_x+1,1)*[n_sq_y:-1:0]*dY,Np,1)'; +Zi = zeros(1,Np); + +Xgrid = [Xi;Yi;Zi]; + +% All the point coordinates (on the image, and in 3D) - for global optimization: +x = grid_pts; +X = Xgrid; + +% Saves all the data into variables: +eval(['dX_' num2str(kk) ' = dX;']); +eval(['dY_' num2str(kk) ' = dY;']); +eval(['wintx_' num2str(kk) ' = wintx;']); +eval(['winty_' num2str(kk) ' = winty;']); +eval(['x_' num2str(kk) ' = x;']); +eval(['X_' num2str(kk) ' = X;']); +eval(['n_sq_x_' num2str(kk) ' = n_sq_x;']); +eval(['n_sq_y_' num2str(kk) ' = n_sq_y;']); diff --git a/src/g_calib/click_ima_calib_no_read.m b/src/g_calib/click_ima_calib_no_read.m new file mode 100755 index 0000000000000000000000000000000000000000..aeba2c5cf755a23f9a6c88f358af007aca241e10 --- /dev/null +++ b/src/g_calib/click_ima_calib_no_read.m @@ -0,0 +1,382 @@ +if ~exist('rosette_calibration', 'var') + rosette_calibration = 0; +end; + +if (rosette_calibration) + % Guesses for the intrinsic parameters. + fg = [ 2300.044453320904 2304.757690230091 ]'; + cg = [924.743157649778 1282.463968890109 ]'; + kg = [-0.443251898487364 0.247922214991804 -0.000682045390384 0.000326391030429 -0.092405396596532]'; +end; + +if exist(['wintx_' num2str(kk)]), + eval(['wintxkk = wintx_' num2str(kk) ';']); + if ~isempty(wintxkk) & ~isnan(wintxkk), + eval(['wintx = wintx_' num2str(kk) ';']); + eval(['winty = winty_' num2str(kk) ';']); + end; +end; + +if (rosette_calibration), + wintx = 20; + winty = 20; +end; + +fprintf(1,'Using (wintx,winty)=(%d,%d) - Window size = %dx%d (Note: To reset the window size, run script clearwin)\n',wintx,winty,2*wintx+1,2*winty+1); + +grid_success = 0; + +while (~grid_success) + bad_clicks = 1; + + while bad_clicks, + + figure(2); clf; + image(I); + colormap(map); + axis image; + set(2,'color',[1 1 1]); + title(['Click on the four extreme corners of the rectangular pattern (first corner = origin)... Image ' num2str(kk)]); + disp('Click on the four extreme corners of the rectangular complete pattern (the first clicked corner is the origin)...'); + + if (rosette_calibration) + position_fig = get(2,'position'); + position_fig(3:4) = [674 824]; + set(2,'position',position_fig); + findfigs; + end; + + wintx_save = wintx; + winty_save = winty; + + if (rosette_calibration) + wintx = 30; + winty = 30; + end; + + x= [];y = []; + figure(2); hold on; + for count = 1:4, + [xi,yi] = ginput4(1); + [xxi] = cornerfinder([xi;yi],I,winty,wintx); + xi = xxi(1); + yi = xxi(2); + figure(2); + plot(xi,yi,'+','color',[ 1.000 0.314 0.510 ],'linewidth',2); + plot(xi + [wintx+.5 -(wintx+.5) -(wintx+.5) wintx+.5 wintx+.5],yi + [winty+.5 winty+.5 -(winty+.5) -(winty+.5) winty+.5],'-','color',[ 1.000 0.314 0.510 ],'linewidth',2); + x = [x;xi]; + y = [y;yi]; + plot(x,y,'-','color',[ 1.000 0.314 0.510 ],'linewidth',2); + drawnow; + end; + plot([x;x(1)],[y;y(1)],'-','color',[ 1.000 0.314 0.510 ],'linewidth',2); + drawnow; + hold off; + + wintx = wintx_save; + winty = winty_save; + + [Xc,good,bad,type] = cornerfinder([x';y'],I,winty,wintx); % the four corners + + bad_clicks = (sum(bad)>0); + + end; + + x = Xc(1,:)'; + y = Xc(2,:)'; + + % Sort the corners: + x_mean = mean(x); + y_mean = mean(y); + x_v = x - x_mean; + y_v = y - y_mean; + + theta = atan2(-y_v,x_v); + [junk,ind] = sort(theta); + + [junk,ind] = sort(mod(theta-theta(1),2*pi)); + + ind = ind([4 3 2 1]); %-> New: the Z axis is pointing uppward + + x = x(ind); + y = y(ind); + + if (rosette_calibration) + % Enforce the ordering convention for the Rosette calibration. + % get the first two points by sorting the x: + [x_junk,ind_sort_x] = sort(x); + first_two_points = ind_sort_x(1:2); + last_two_points = ind_sort_x(3:4); + + [y_junk, ind_sort_y] = sort(y(first_two_points)); + ind1 = first_two_points(ind_sort_y(2)); + ind2 = first_two_points(ind_sort_y(1)); + [y_junk, ind_sort_y] = sort(y(last_two_points)); + ind3 = last_two_points(ind_sort_y(1)); + ind4 = last_two_points(ind_sort_y(2)); + + ind_resort = [ind1;ind2;ind3;ind4]; + x = x(ind_resort); + y = y(ind_resort); + end; + + x1= x(1); x2 = x(2); x3 = x(3); x4 = x(4); + y1= y(1); y2 = y(2); y3 = y(3); y4 = y(4); + + % Find center: + p_center = cross(cross([x1;y1;1],[x3;y3;1]),cross([x2;y2;1],[x4;y4;1])); + x5 = p_center(1)/p_center(3); + y5 = p_center(2)/p_center(3); + + % center on the X axis: + x6 = (x3 + x4)/2; + y6 = (y3 + y4)/2; + + % center on the Y axis: + x7 = (x1 + x4)/2; + y7 = (y1 + y4)/2; + + % Direction of displacement for the X axis: + vX = [x6-x5;y6-y5]; + vX = vX / norm(vX); + + % Direction of displacement for the X axis: + vY = [x7-x5;y7-y5]; + vY = vY / norm(vY); + + % Direction of diagonal: + vO = [x4 - x5; y4 - y5]; + vO = vO / norm(vO); + + delta = 30; + + figure(2); image(I); + axis image; + colormap(map); + hold on; + plot([x;x(1)],[y;y(1)],'g-'); + plot(x,y,'og'); + hx=text(x6 + delta * vX(1) ,y6 + delta*vX(2),'X'); + set(hx,'color','g','Fontsize',14); + hy=text(x7 + delta*vY(1), y7 + delta*vY(2),'Y'); + set(hy,'color','g','Fontsize',14); + hO=text(x4 + delta * vO(1) ,y4 + delta*vO(2),'O','color','g','Fontsize',14); + for iii = 1:4, + text(x(iii),y(iii),num2str(iii)); + end; + hold off; + + if manual_squares, + n_sq_x = input(['Number of squares along the X direction ([]=' num2str(n_sq_x_default) ') = ']); %6 + if isempty(n_sq_x), n_sq_x = n_sq_x_default; end; + n_sq_y = input(['Number of squares along the Y direction ([]=' num2str(n_sq_y_default) ') = ']); %6 + if isempty(n_sq_y), n_sq_y = n_sq_y_default; end; + grid_success = 1; + else + if (rosette_calibration) + n_sq_x1 = count_squares_distorted(I,x1,y1,x2,y2,wintx, fg, cg, kg); + n_sq_x2 = count_squares_distorted(I,x3,y3,x4,y4,wintx, fg, cg, kg); + n_sq_y1 = count_squares_distorted(I,x2,y2,x3,y3,wintx, fg, cg, kg); + n_sq_y2 = count_squares_distorted(I,x4,y4,x1,y1,wintx, fg, cg, kg); + else + % Try to automatically count the number of squares in the grid + n_sq_x1 = count_squares(I,x1,y1,x2,y2,wintx); + n_sq_x2 = count_squares(I,x3,y3,x4,y4,wintx); + n_sq_y1 = count_squares(I,x2,y2,x3,y3,wintx); + n_sq_y2 = count_squares(I,x4,y4,x1,y1,wintx); + end; + + % If could not count the number of squares, enter manually + if ((n_sq_x1~=n_sq_x2)||(n_sq_y1~=n_sq_y2)||... + (min([n_sq_x1 n_sq_x2 n_sq_y1 n_sq_y2] < 0))), + if (rosette_calibration) + % Try harder using an existing intrinsic model: + n_sq_x1 = count_squares_distorted(I,x1,y1,x2,y2,wintx, fg, cg, kg); + n_sq_x2 = count_squares_distorted(I,x3,y3,x4,y4,wintx, fg, cg, kg); + n_sq_y1 = count_squares_distorted(I,x2,y2,x3,y3,wintx, fg, cg, kg); + n_sq_y2 = count_squares_distorted(I,x4,y4,x1,y1,wintx, fg, cg, kg); + end; + if ((n_sq_x1~=n_sq_x2)||(n_sq_y1~=n_sq_y2)||... + (min([n_sq_x1 n_sq_x2 n_sq_y1 n_sq_y2] < 0))), + if ~rosette_calibration, + disp('Could not count the number of squares in the grid. Enter manually.'); + n_sq_x = input(['Number of squares along the X direction ([]=' num2str(n_sq_x_default) ') = ']); %6 + if isempty(n_sq_x), n_sq_x = n_sq_x_default; end; + n_sq_y = input(['Number of squares along the Y direction ([]=' num2str(n_sq_y_default) ') = ']); %6 + if isempty(n_sq_y), n_sq_y = n_sq_y_default; end; + grid_success = 1; + else + grid_success = 0; + end; + else + n_sq_x = n_sq_x1; + n_sq_y = n_sq_y1; + grid_success = 1; + end; + else + n_sq_x = n_sq_x1; + n_sq_y = n_sq_y1; + grid_success = 1; + end; + end; + if ~grid_success + fprintf(1,'Invalid grid. Try again.\n'); + end; +end; + +n_sq_x_default = n_sq_x; +n_sq_y_default = n_sq_y; + + +if (exist('dX')~=1)||(exist('dY')~=1), % This question is now asked only once + % Enter the size of each square + dX = input(['Size dX of each square along the X direction ([]=' num2str(dX_default) 'm) = ']); + dY = input(['Size dY of each square along the Y direction ([]=' num2str(dY_default) 'm) = ']); + if isempty(dX), dX = dX_default; else dX_default = dX; end; + if isempty(dY), dY = dY_default; else dY_default = dY; end; +else + fprintf(1,['Size of each square along the X direction: dX=' num2str(dX) 'm\n']); + fprintf(1,['Size of each square along the Y direction: dY=' num2str(dY) 'm (Note: To reset the size of the squares, clear the variables dX and dY)\n']); +end; + +if (~rosette_calibration) + % Compute the inside points through computation of the planar homography (collineation) + a00 = [x(1);y(1);1]; + a10 = [x(2);y(2);1]; + a11 = [x(3);y(3);1]; + a01 = [x(4);y(4);1]; + + % Compute the planar collineation: (return the normalization matrix as well) + [Homo,Hnorm,inv_Hnorm] = compute_homography([a00 a10 a11 a01],[0 1 1 0;0 0 1 1;1 1 1 1]); + + % Build the grid using the planar collineation: + x_l = ((0:n_sq_x)'*ones(1,n_sq_y+1))/n_sq_x; + y_l = (ones(n_sq_x+1,1)*(0:n_sq_y))/n_sq_y; + pts = [x_l(:) y_l(:) ones((n_sq_x+1)*(n_sq_y+1),1)]'; + XX = Homo*pts; + XX = XX(1:2,:) ./ (ones(2,1)*XX(3,:)); +else + [x_pn] = normalize_pixel([x' ; y'] - 1,fg,cg,kg,0); + + % Compute the inside points through computation of the planar homography (collineation) + a00 = [x_pn(1,1);x_pn(2,1);1]; + a10 = [x_pn(1,2);x_pn(2,2);1]; + a11 = [x_pn(1,3);x_pn(2,3);1]; + a01 = [x_pn(1,4);x_pn(2,4);1]; + + % Compute the planar collineation: (return the normalization matrix as well) + [Homo,Hnorm,inv_Hnorm] = compute_homography([a00 a10 a11 a01],[0 1 1 0;0 0 1 1;1 1 1 1]); + + % Build the grid using the planar collineation: + x_l = ((0:n_sq_x)'*ones(1,n_sq_y+1))/n_sq_x; + y_l = (ones(n_sq_x+1,1)*(0:n_sq_y))/n_sq_y; + pts = [x_l(:) y_l(:) ones((n_sq_x+1)*(n_sq_y+1),1)]'; + + XXpn = Homo*pts; + XXpn = XXpn(1:2,:) ./ (ones(2,1)*XXpn(3,:)); + + XX = apply_distortion2(XXpn,kg); + + XX(1,:) = fg(1)*XX(1,:) + cg(1) + 1; + XX(2,:) = fg(2)*XX(2,:) + cg(2) + 1; +end; + +% Complete size of the rectangle +W = n_sq_x*dX; +L = n_sq_y*dY; + +if (~rosette_calibration) +%%%%%%%%%%%%%%%%%%%%%%%% ADDITIONAL STUFF IN THE CASE OF HIGHLY DISTORTED IMAGES %%%%%%%%%%%%% +figure(2); +hold on; +plot(XX(1,:),XX(2,:),'r+'); +title('The red crosses should be close to the image corners'); +hold off; + +disp('If the guessed grid corners (red crosses on the image) are not close to the actual corners,'); +disp('it is necessary to enter an initial guess for the radial distortion factor kc (useful for subpixel detection)'); +quest_distort = input('Need of an initial guess for distortion? ([]=no, other=yes) '); + +quest_distort = ~isempty(quest_distort); + +if quest_distort, + % Estimation of focal length: + c_g = [size(I,2);size(I,1)]/2 + .5; + f_g = Distor2Calib(0,[[x(1) x(2) x(4) x(3)] - c_g(1);[y(1) y(2) y(4) y(3)] - c_g(2)],1,1,4,W,L,[-W/2 W/2 W/2 -W/2;L/2 L/2 -L/2 -L/2; 0 0 0 0],100,1,1); + f_g = mean(f_g); + script_fit_distortion; +end; +%%%%%%%%%%%%%%%%%%%%% END ADDITIONAL STUFF IN THE CASE OF HIGHLY DISTORTED IMAGES %%%%%%%%%%%%% + +end; + + + +Np = (n_sq_x+1)*(n_sq_y+1); + +disp('Corner extraction...'); + +grid_pts = cornerfinder(XX,I,winty,wintx); %%% Finds the exact corners at every points! + +%save all_corners x y grid_pts + +grid_pts = grid_pts - 1; % subtract 1 to bring the origin to (0,0) instead of (1,1) in matlab (not necessary in C) + +ind_corners = [1 n_sq_x+1 (n_sq_x+1)*n_sq_y+1 (n_sq_x+1)*(n_sq_y+1)]; % index of the 4 corners +ind_orig = (n_sq_x+1)*n_sq_y + 1; +xorig = grid_pts(1,ind_orig); +yorig = grid_pts(2,ind_orig); +dxpos = mean([grid_pts(:,ind_orig) grid_pts(:,ind_orig+1)]'); +dypos = mean([grid_pts(:,ind_orig) grid_pts(:,ind_orig-n_sq_x-1)]'); + + +x_box_kk = [grid_pts(1,:)-(wintx+.5);grid_pts(1,:)+(wintx+.5);grid_pts(1,:)+(wintx+.5);grid_pts(1,:)-(wintx+.5);grid_pts(1,:)-(wintx+.5)]; +y_box_kk = [grid_pts(2,:)-(winty+.5);grid_pts(2,:)-(winty+.5);grid_pts(2,:)+(winty+.5);grid_pts(2,:)+(winty+.5);grid_pts(2,:)-(winty+.5)]; + + +figure(3); +image(I); axis image; colormap(map); hold on; +plot(grid_pts(1,:)+1,grid_pts(2,:)+1,'r+'); +plot(x_box_kk+1,y_box_kk+1,'-b'); +plot(grid_pts(1,ind_corners)+1,grid_pts(2,ind_corners)+1,'mo'); +plot(xorig+1,yorig+1,'*m'); +h = text(xorig+delta*vO(1),yorig+delta*vO(2),'O'); +set(h,'Color','m','FontSize',14); +h2 = text(dxpos(1)+delta*vX(1),dxpos(2)+delta*vX(2),'dX'); +set(h2,'Color','g','FontSize',14); +h3 = text(dypos(1)+delta*vY(1),dypos(2)+delta*vY(2),'dY'); +set(h3,'Color','g','FontSize',14); +xlabel('Xc (in camera frame)'); +ylabel('Yc (in camera frame)'); +title('Extracted corners'); +zoom on; +drawnow; +hold off; + + +Xi = reshape(([0:n_sq_x]*dX)'*ones(1,n_sq_y+1),Np,1)'; +Yi = reshape(ones(n_sq_x+1,1)*[n_sq_y:-1:0]*dY,Np,1)'; +Zi = zeros(1,Np); + +Xgrid = [Xi;Yi;Zi]; + + +% All the point coordinates (on the image, and in 3D) - for global optimization: + +x = grid_pts; +X = Xgrid; + + +% Saves all the data into variables: + +eval(['dX_' num2str(kk) ' = dX;']); +eval(['dY_' num2str(kk) ' = dY;']); + +eval(['wintx_' num2str(kk) ' = wintx;']); +eval(['winty_' num2str(kk) ' = winty;']); + +eval(['x_' num2str(kk) ' = x;']); +eval(['X_' num2str(kk) ' = X;']); + +eval(['n_sq_x_' num2str(kk) ' = n_sq_x;']); +eval(['n_sq_y_' num2str(kk) ' = n_sq_y;']); diff --git a/src/g_calib/click_stereo.m b/src/g_calib/click_stereo.m new file mode 100755 index 0000000000000000000000000000000000000000..8c4b5690a481e9f9e55d89d6dbefd83cc517f22e --- /dev/null +++ b/src/g_calib/click_stereo.m @@ -0,0 +1,53 @@ +function [xL,xR] = click_stereo(NUMBER_OF_POINTS,IL,IR,R,T,fc_right,cc_right,kc_right,alpha_c_right,fc_left,cc_left,kc_left,alpha_c_left); + + +figure(1); +image(IL); + +figure(2); +image(IR); + +[ny,nx] = size(IL); + +xL = []; +xR = []; + +for kk = 1:NUMBER_OF_POINTS, + + figure(1); + hold on; + x = ginput(1); + plot(x(1),x(2),'g.'); + hold off; + x = x'-1; + + xL = [xL x]; + + [epipole] = compute_epipole(x,R,T,fc_right,cc_right,kc_right,alpha_c_right,fc_left,cc_left,kc_left,alpha_c_left); + + figure(2); + hold on; + h = plot(epipole(1,:)+1,epipole(2,:)+1,'r.','markersize',1); + hold off; + + x2 = ginput(1); + x2 = x2' - 1; + + NN = size(epipole,2); + d = sum((epipole - repmat(x2,1,NN)).^2); + [junk,indmin] = min(d); + + x2 = epipole(:,indmin); + + xR = [xR x2]; + + delete(h); + + figure(2); + hold on; + plot(x2(1)+1,x2(2)+1,'g.'); + drawnow; + hold off; + +end; + diff --git a/src/g_calib/combine_calib.m b/src/g_calib/combine_calib.m new file mode 100755 index 0000000000000000000000000000000000000000..277c11d2ece8454dbcc87b815b4ccdf31a8cbb69 --- /dev/null +++ b/src/g_calib/combine_calib.m @@ -0,0 +1,40 @@ +%%% Script that combines two calibration sets together. + + +dir; + +name1 = input('Calibration file name #1: ','s'); +name2 = input('Calibration file name #2: ','s'); + + +load(name1); + +n_ima_1 = n_ima; + + +load(name2); + +n_ima_2= n_ima; +active_images_2 = active_images; + +for kk=n_ima:-1:1, + + eval(['X_' num2str(kk+n_ima_1) '=X_' num2str(kk) ';' ]); + eval(['x_' num2str(kk+n_ima_1) '=x_' num2str(kk) ';' ]); + eval(['dX_' num2str(kk+n_ima_1) '=dX_' num2str(kk) ';' ]); + eval(['dY_' num2str(kk+n_ima_1) '=dY_' num2str(kk) ';' ]); + eval(['n_sq_x_' num2str(kk+n_ima_1) '=n_sq_x_' num2str(kk) ';' ]); + eval(['n_sq_y_' num2str(kk+n_ima_1) '=n_sq_y_' num2str(kk) ';' ]); + eval(['wintx_' num2str(kk+n_ima_1) '=wintx_' num2str(kk) ';' ]); + eval(['winty_' num2str(kk+n_ima_1) '=winty_' num2str(kk) ';' ]); + +end; + +load(name1); + +n_ima = n_ima + n_ima_2; +active_images = [ active_images active_images_2]; + +%no_image = 1; + +clear calib_name format_image type_numbering image_numbers N_slots \ No newline at end of file diff --git a/src/g_calib/comp_distortion.m b/src/g_calib/comp_distortion.m new file mode 100755 index 0000000000000000000000000000000000000000..6e757bcf0a845d694edfa7aa7f6ff29cab8e0ef5 --- /dev/null +++ b/src/g_calib/comp_distortion.m @@ -0,0 +1,43 @@ +function [x_comp] = comp_distortion(x_dist,k2); + +% [x_comp] = comp_distortion(x_dist,k2); +% +% compensates the radial distortion of the camera +% on the image plane. +% +% x_dist : the image points got without considering the +% radial distortion. +% x : The image plane points after correction for the distortion +% +% x and x_dist are 2xN arrays +% +% NOTE : This compensation has to be done after the substraction +% of the center of projection, and division by the focal +% length. +% +% (do it up to a second order approximation) + + +[two,N] = size(x_dist); + + +if (two ~= 2 ), + error('ERROR : The dimension of the points should be 2xN'); +end; + + +if length(k2) > 1, + + [x_comp] = comp_distortion_oulu(x_dist,k2); + +else + + radius_2= x_dist(1,:).^2 + x_dist(2,:).^2; + radial_distortion = 1 + ones(2,1)*(k2 * radius_2); + radius_2_comp = (x_dist(1,:).^2 + x_dist(2,:).^2) ./ radial_distortion(1,:); + radial_distortion = 1 + ones(2,1)*(k2 * radius_2_comp); + x_comp = x_dist ./ radial_distortion; + +end; + +%% Function completely checked : It works fine !!! \ No newline at end of file diff --git a/src/g_calib/comp_distortion2.m b/src/g_calib/comp_distortion2.m new file mode 100755 index 0000000000000000000000000000000000000000..e2f70ecf51124313027709cff01c338bd4b5dfa1 --- /dev/null +++ b/src/g_calib/comp_distortion2.m @@ -0,0 +1,73 @@ +function [x_comp] = comp_distortion(x_dist,k2); + +% [x_comp] = comp_distortion(x_dist,k2); +% +% compensates the radial distortion of the camera +% on the image plane. +% +% x_dist : the image points got without considering the +% radial distortion. +% k2: Radial distortion factor +% +% x : The image plane points after correction for the distortion +% +% x and x_dist are 2xN arrays +% +% NOTE : This compensation has to be done after the substraction +% of the center of projection, and division by the focal +% length. +% +% Solve for cubic roots using method from Numerical Recipes in C 2nd Ed. +% pages 184-185. + + +% California Institute of Technology +% (c) Jean-Yves Bouguet - April 27th, 1998 + +% fully checked! JYB, april 27th, 1998 - 2am + +if k2 ~= 0, + +[two,N] = size(x_dist); + +if (two ~= 2 ), + error('ERROR : The dimension of the points should be 2xN'); +end; + + +ph = atan2(x_dist(2,:),x_dist(1,:)); + +Q = -1/(3*k2); +R = -x_dist(1,:)./(2*k2*cos(ph)); + +R2 = R.^2; +Q3 = Q^3; + + +if k2 < 0, + + % this works in all practical situations (it starts failing for very large + % values of k2) + + th = acos(R./sqrt(Q3)); + r = -2*sqrt(Q)*cos((th-2*pi)/3); + +else + + % note: this always works, even for ridiculous values of k2 + + A = (sqrt(R2-Q3)-R).^(1/3); + B = Q*(1./A); + r = (A+B); + +end; + +x_comp = [r.*cos(ph); r.*sin(ph)]; + +else + + x_comp = x_dist; + +end; + +x_comp = real(x_comp); diff --git a/src/g_calib/comp_distortion_oulu.m b/src/g_calib/comp_distortion_oulu.m new file mode 100755 index 0000000000000000000000000000000000000000..1002036574fe5fc7bc500e291cd7a97aed98db1e --- /dev/null +++ b/src/g_calib/comp_distortion_oulu.m @@ -0,0 +1,48 @@ +function [x] = comp_distortion_oulu(xd,k); + +%comp_distortion_oulu.m +% +%[x] = comp_distortion_oulu(xd,k) +% +%Compensates for radial and tangential distortion. Model From Oulu university. +%For more informatino about the distortion model, check the forward projection mapping function: +%project_points.m +% +%INPUT: xd: distorted (normalized) point coordinates in the image plane (2xN matrix) +% k: Distortion coefficients (radial and tangential) (4x1 vector) +% +%OUTPUT: x: undistorted (normalized) point coordinates in the image plane (2xN matrix) +% +%Method: Iterative method for compensation. +% +%NOTE: This compensation has to be done after the subtraction +% of the principal point, and division by the focal length. + + +if length(k) == 1, + + [x] = comp_distortion(xd,k); + +else + + k1 = k(1); + k2 = k(2); + k3 = k(5); + p1 = k(3); + p2 = k(4); + + x = xd; % initial guess + + for kk=1:20, + + r_2 = sum(x.^2); + k_radial = 1 + k1 * r_2 + k2 * r_2.^2 + k3 * r_2.^3; + delta_x = [2*p1*x(1,:).*x(2,:) + p2*(r_2 + 2*x(1,:).^2); + p1 * (r_2 + 2*x(2,:).^2)+2*p2*x(1,:).*x(2,:)]; + x = (xd - delta_x)./(ones(2,1)*k_radial); + + end; + +end; + + \ No newline at end of file diff --git a/src/g_calib/comp_error_calib.m b/src/g_calib/comp_error_calib.m new file mode 100755 index 0000000000000000000000000000000000000000..b88d454a88223f481c559f9efa8ac9dd1511adf4 --- /dev/null +++ b/src/g_calib/comp_error_calib.m @@ -0,0 +1,46 @@ +%%%%%%%%%%%%%%%%%%%% RECOMPUTES THE REPROJECTION ERROR %%%%%%%%%%%%%%%%%%%%%%%% + +check_active_images; + +% Reproject the patterns on the images, and compute the pixel errors: + +ex = []; % Global error vector +x = []; % Detected corners on the image plane +y = []; % Reprojected points + +if ~exist('alpha_c'), + alpha_c = 0; +end; + +for kk = 1:n_ima, + + eval(['omckk = omc_' num2str(kk) ';']); + eval(['Tckk = Tc_' num2str(kk) ';']); + + if active_images(kk) & (~isnan(omckk(1,1))), + + %Rkk = rodrigues(omckk); + + eval(['y_' num2str(kk) ' = project_points2(X_' num2str(kk) ',omckk,Tckk,fc,cc,kc,alpha_c);']); + + eval(['ex_' num2str(kk) ' = x_' num2str(kk) ' - y_' num2str(kk) ';']); + + eval(['x_kk = x_' num2str(kk) ';']); + + eval(['ex = [ex ex_' num2str(kk) '];']); + eval(['x = [x x_' num2str(kk) '];']); + eval(['y = [y y_' num2str(kk) '];']); + + else + + % eval(['y_' num2str(kk) ' = NaN*ones(2,1);']); + + + % If inactivated image, the error does not make sense: + eval(['ex_' num2str(kk) ' = NaN*ones(2,1);']); + + end; + +end; + +err_std = std(ex')'; diff --git a/src/g_calib/comp_error_calib_fisheye.m b/src/g_calib/comp_error_calib_fisheye.m new file mode 100755 index 0000000000000000000000000000000000000000..f0cb4ef9fcf612b7194621ca0463804ccbfa4529 --- /dev/null +++ b/src/g_calib/comp_error_calib_fisheye.m @@ -0,0 +1,46 @@ +%%%%%%%%%%%%%%%%%%%% RECOMPUTES THE REPROJECTION ERROR %%%%%%%%%%%%%%%%%%%%%%%% + +check_active_images; + +% Reproject the patterns on the images, and compute the pixel errors: + +ex = []; % Global error vector +x = []; % Detected corners on the image plane +y = []; % Reprojected points + +if ~exist('alpha_c'), + alpha_c = 0; +end; + +for kk = 1:n_ima, + + eval(['omckk = omc_' num2str(kk) ';']); + eval(['Tckk = Tc_' num2str(kk) ';']); + + if active_images(kk) & (~isnan(omckk(1,1))), + + %Rkk = rodrigues(omckk); + + eval(['y_' num2str(kk) ' = project_points_fisheye(X_' num2str(kk) ',omckk,Tckk,fc,cc,kc,alpha_c);']); + + eval(['ex_' num2str(kk) ' = x_' num2str(kk) ' - y_' num2str(kk) ';']); + + eval(['x_kk = x_' num2str(kk) ';']); + + eval(['ex = [ex ex_' num2str(kk) '];']); + eval(['x = [x x_' num2str(kk) '];']); + eval(['y = [y y_' num2str(kk) '];']); + + else + + % eval(['y_' num2str(kk) ' = NaN*ones(2,1);']); + + + % If inactivated image, the error does not make sense: + eval(['ex_' num2str(kk) ' = NaN*ones(2,1);']); + + end; + +end; + +err_std = std(ex')'; diff --git a/src/g_calib/comp_ext_calib.m b/src/g_calib/comp_ext_calib.m new file mode 100755 index 0000000000000000000000000000000000000000..3400f575c6dd3832f3d67118d207256b1f7dc686 --- /dev/null +++ b/src/g_calib/comp_ext_calib.m @@ -0,0 +1,62 @@ +%%% Computes the extrinsic parameters for all the active calibration images + +check_active_images; + +N_points_views = zeros(1,n_ima); + +for kk = 1:n_ima, + + if exist(['x_' num2str(kk)]), + + eval(['x_kk = x_' num2str(kk) ';']); + eval(['X_kk = X_' num2str(kk) ';']); + + if (isnan(x_kk(1,1))), + if active_images(kk), + fprintf(1,'Warning: Cannot calibrate with image %d. Need to extract grid corners first.\n',kk) + fprintf(1,' Set active_images(%d)=1; and run Extract grid corners.\n',kk) + end; + end; + if active_images(kk), + N_points_views(kk) = size(x_kk,2); + [omckk,Tckk] = compute_extrinsic_init(x_kk,X_kk,fc,cc,kc,alpha_c); + [omckk,Tckk,Rckk,JJ_kk] = compute_extrinsic_refine(omckk,Tckk,x_kk,X_kk,fc,cc,kc,alpha_c,20,thresh_cond); + if check_cond, + if (cond(JJ_kk)> thresh_cond), + active_images(kk) = 0; + omckk = NaN*ones(3,1); + Tckk = NaN*ones(3,1); + fprintf(1,'\nWarning: View #%d ill-conditioned. This image is now set inactive.\n',kk) + desactivated_images = [desactivated_images kk]; + end; + end; + if isnan(omckk(1,1)), + %fprintf(1,'\nWarning: Desactivating image %d. Re-activate it later by typing:\nactive_images(%d)=1;\nand re-run optimization\n',[kk kk]) + active_images(kk) = 0; + end; + else + omckk = NaN*ones(3,1); + Tckk = NaN*ones(3,1); + end; + + else + + omckk = NaN*ones(3,1); + Tckk = NaN*ones(3,1); + + if active_images(kk), + fprintf(1,'Warning: Cannot calibrate with image %d. Need to extract grid corners first.\n',kk) + fprintf(1,' Set active_images(%d)=1; and run Extract grid corners.\n',kk) + end; + + active_images(kk) = 0; + + end; + + eval(['omc_' num2str(kk) ' = omckk;']); + eval(['Tc_' num2str(kk) ' = Tckk;']); + +end; + + +check_active_images; diff --git a/src/g_calib/comp_ext_calib_fisheye.m b/src/g_calib/comp_ext_calib_fisheye.m new file mode 100755 index 0000000000000000000000000000000000000000..6b61c2c2b2eabc41bb07eb0de560f02dc0baf019 --- /dev/null +++ b/src/g_calib/comp_ext_calib_fisheye.m @@ -0,0 +1,62 @@ +%%% Computes the extrinsic parameters for all the active calibration images + +check_active_images; + +N_points_views = zeros(1,n_ima); + +for kk = 1:n_ima, + + if exist(['x_' num2str(kk)]), + + eval(['x_kk = x_' num2str(kk) ';']); + eval(['X_kk = X_' num2str(kk) ';']); + + if (isnan(x_kk(1,1))), + if active_images(kk), + fprintf(1,'Warning: Cannot calibrate with image %d. Need to extract grid corners first.\n',kk) + fprintf(1,' Set active_images(%d)=1; and run Extract grid corners.\n',kk) + end; + end; + if active_images(kk), + N_points_views(kk) = size(x_kk,2); + [omckk,Tckk] = compute_extrinsic_init_fisheye(x_kk,X_kk,fc,cc,kc,alpha_c); + [omckk,Tckk,Rckk,JJ_kk] = compute_extrinsic_refine_fisheye(omckk,Tckk,x_kk,X_kk,fc,cc,kc,alpha_c,20,thresh_cond); + if check_cond, + if (cond(JJ_kk)> thresh_cond), + active_images(kk) = 0; + omckk = NaN*ones(3,1); + Tckk = NaN*ones(3,1); + fprintf(1,'\nWarning: View #%d ill-conditioned. This image is now set inactive.\n',kk) + desactivated_images = [desactivated_images kk]; + end; + end; + if isnan(omckk(1,1)), + %fprintf(1,'\nWarning: Desactivating image %d. Re-activate it later by typing:\nactive_images(%d)=1;\nand re-run optimization\n',[kk kk]) + active_images(kk) = 0; + end; + else + omckk = NaN*ones(3,1); + Tckk = NaN*ones(3,1); + end; + + else + + omckk = NaN*ones(3,1); + Tckk = NaN*ones(3,1); + + if active_images(kk), + fprintf(1,'Warning: Cannot calibrate with image %d. Need to extract grid corners first.\n',kk) + fprintf(1,' Set active_images(%d)=1; and run Extract grid corners.\n',kk) + end; + + active_images(kk) = 0; + + end; + + eval(['omc_' num2str(kk) ' = omckk;']); + eval(['Tc_' num2str(kk) ' = Tckk;']); + +end; + + +check_active_images; diff --git a/src/g_calib/comp_fisheye_distortion.m b/src/g_calib/comp_fisheye_distortion.m new file mode 100755 index 0000000000000000000000000000000000000000..88439f19ecbfce6503a07867e9732bf3ad8491fa --- /dev/null +++ b/src/g_calib/comp_fisheye_distortion.m @@ -0,0 +1,49 @@ +function [x] = comp_fisheye_distortion(xd,k) + +%comp_fisheye_distortion.m +% +%[x] = comp_fisheye_distortion(xd,k) +% +%Compensates for fisheye distortions +% +%INPUT: xd: distorted (normalized) point coordinates in the image plane (2xN matrix) +% k: Fisheye distortion coefficients (5x1 vector) +% +%OUTPUT: x: undistorted (normalized) point coordinates in the image plane (2xN matrix) +% +%Method: Iterative method for compensation. +% +%NOTE: This compensation has to be done after the subtraction +% of the principal point, and division by the focal length. + +theta_d = sqrt(xd(1,:).^2 + xd(2,:).^2); +theta = theta_d; % initial guess +for kk=1:20, + theta = theta_d ./ (1 + k(1)*theta.^2 + k(2)*theta.^4 + k(3)*theta.^6 + k(4)*theta.^8); +end; +scaling = tan(theta) ./ theta_d; + +x = xd .* (ones(2,1)*scaling); + +return; + +% Test + +n = 4; +xxx = rand(2,n); + +xxx = [[1.0840 0.3152 0.2666 0.9347 ];[ 0.7353 0.6101 -0.6415 -0.8006]]; + +k = 0.00 * randn(4,1); + +[xd] = comp_fisheye_distortion(xxx,k); +x2 = apply_fisheye_distortion(xd,k); +norm(x2-xd)/norm(x2-xxx) + + +%[xd] = apply_fisheye_distortion(xxx,k); +%x2 = comp_fisheye_distortion(xd,k); +%norm(x2-xd)/norm(x2-xxx) + + + diff --git a/src/g_calib/compose_motion.m b/src/g_calib/compose_motion.m new file mode 100755 index 0000000000000000000000000000000000000000..eb19582883d3f41ae4995ea83a15e71bb22fe908 --- /dev/null +++ b/src/g_calib/compose_motion.m @@ -0,0 +1,62 @@ +function [om3,T3,dom3dom1,dom3dT1,dom3dom2,dom3dT2,dT3dom1,dT3dT1,dT3dom2,dT3dT2] = compose_motion(om1,T1,om2,T2); + +% Rotations: + +[R1,dR1dom1] = rodrigues(om1); +[R2,dR2dom2] = rodrigues(om2); + +R3 = R2 * R1; + +[dR3dR2,dR3dR1] = dAB(R2,R1); + +[om3,dom3dR3] = rodrigues(R3); + +dom3dom1 = dom3dR3 * dR3dR1 * dR1dom1; +dom3dom2 = dom3dR3 * dR3dR2 * dR2dom2; + +dom3dT1 = zeros(3,3); +dom3dT2 = zeros(3,3); + + +% Translations: + +T3t = R2 * T1; + +[dT3tdR2,dT3tdT1] = dAB(R2,T1); + +dT3tdom2 = dT3tdR2 * dR2dom2; + + +T3 = T3t + T2; + +dT3dT1 = dT3tdT1; +dT3dT2 = eye(3); + +dT3dom2 = dT3tdom2; +dT3dom1 = zeros(3,3); + + +return; + +% Test of the Jacobians: + +om1 = randn(3,1); +om2 = randn(3,1); +T1 = 10*randn(3,1); +T2 = 10*randn(3,1); + +[om3,T3,dom3dom1,dom3dT1,dom3dom2,dom3dT2,dT3dom1,dT3dT1,dT3dom2,dT3dT2] = compose_motion(om1,T1,om2,T2); + + +dom1 = randn(3,1) / 1000; +dom2 = randn(3,1) / 1000; +dT1 = randn(3,1) / 10000; +dT2 = randn(3,1) / 10000; + +[om3r,T3r] = compose_motion(om1+dom1,T1+dT1,om2+dom2,T2+dT2); + +om3p = om3 + dom3dom1*dom1 + dom3dT1*dT1 + dom3dom2*dom2 + dom3dT2*dT2; +T3p = T3 + dT3dom1*dom1 + dT3dT1*dT1 + dT3dom2*dom2 + dT3dT2*dT2; + +norm(om3r - om3) / norm(om3r - om3p) +norm(T3r - T3) / norm(T3r - T3p) diff --git a/src/g_calib/compute_collineation.m b/src/g_calib/compute_collineation.m new file mode 100755 index 0000000000000000000000000000000000000000..809c3096a572ff74ed406bcae731fb8b5c37ea95 --- /dev/null +++ b/src/g_calib/compute_collineation.m @@ -0,0 +1,66 @@ +function [H,Hnorm,inv_Hnorm] = compute_collineation (a00, a10, a11, a01); + +% new formalism using homographies + +a00 = a00 / a00(3); +a10 = a10 / a10(3); +a11 = a11 / a11(3); +a01 = a01 / a01(3); + + +% Prenormalization of point coordinates (very important): +% (Affine normalization) + +ax = [a00(1);a10(1);a11(1);a01(1)]; +ay = [a00(2);a10(2);a11(2);a01(2)]; + +mxx = mean(ax); +myy = mean(ay); +ax = ax - mxx; +ay = ay - myy; + +scxx = mean(abs(ax)); +scyy = mean(abs(ay)); + + +Hnorm = [1/scxx 0 -mxx/scxx;0 1/scyy -myy/scyy;0 0 1]; +inv_Hnorm = [scxx 0 mxx ; 0 scyy myy; 0 0 1]; + + +a00n = Hnorm*a00; +a10n = Hnorm*a10; +a11n = Hnorm*a11; +a01n = Hnorm*a01; + + +% Computation of the vanishing points: + +V1n = cross(cross(a00n,a10n),cross(a01n,a11n)); +V2n = cross(cross(a00n,a01n),cross(a10n,a11n)); + +V1 = inv_Hnorm*V1n; +V2 = inv_Hnorm*V2n; + + +% Normalizaion of the vanishing points: + +V1n = V1n/norm(V1n); +V2n = V2n/norm(V2n); + + +% Closed-form solution of the coefficients: + +alpha_x = (a10n(2)*a00n(1) - a10n(1)*a00n(2))/(V1n(2)*a10n(1)-V1n(1)*a10n(2)); + +alpha_y = (a01n(2)*a00n(1) - a01n(1)*a00n(2))/(V2n(2)*a01n(1)-V2n(1)*a01n(2)); + + +% Remaining Homography + +Hrem = [alpha_x*V1n alpha_y*V2n a00n]; + + +% Final homography: + +H = inv_Hnorm*Hrem; + diff --git a/src/g_calib/compute_epipole.m b/src/g_calib/compute_epipole.m new file mode 100755 index 0000000000000000000000000000000000000000..e013dcd6666efc91991c0ae35f59f8b7f0de4fc2 --- /dev/null +++ b/src/g_calib/compute_epipole.m @@ -0,0 +1,57 @@ +function [epipole] = compute_epipole(xLp,R,T,fc_right,cc_right,kc_right,alpha_c_right,fc_left,cc_left,kc_left,alpha_c_left,D); + +if ~exist('D'), + D = 400; +end; + +uo = [ normalize_pixel(xLp,fc_left,cc_left,kc_left,alpha_c_left); 1 ]; + +S = [ 0 -T(3) T(2) + T(3) 0 -T(1) + -T(2) T(1) 0 ]; + +l_epipole = (S*R)*uo; + +KK_right = [fc_right(1) alpha_c_right * fc_right(1) cc_right(1) ; 0 fc_right(2) cc_right(2) ; 0 0 1]; + +N_line = 800; + +if norm(l_epipole(2)) > norm(l_epipole(1)), + + % Horizontal line: + + limit_x_pos = ((xLp(1) + D/2) - cc_right(1)) / fc_right(1); + limit_x_neg = ((xLp(1) - D/2) - cc_right(1)) / fc_right(1); + + + %limit_x_pos = ((nx-1) - cc_right(1)) / fc_right(1); + %limit_x_neg = (0 - cc_right(1)) / fc_right(1); + + x_list = (limit_x_pos - limit_x_neg) * ((0:(N_line-1)) / (N_line-1)) + limit_x_neg; + + + pt = cross(repmat(l_epipole,1,N_line),[ones(1,N_line);zeros(1,N_line);-x_list]); + + +else + + limit_y_pos = ((xLp(2) + D/2) - cc_right(2)) / fc_right(2); + limit_y_neg = ((xLp(2) - D/2) - cc_right(2)) / fc_right(2); + + %limit_y_pos = ((ny-1) - cc_right(2)) / fc_right(2); + %limit_y_neg = (0 - cc_right(2)) / fc_right(2); + + y_list = (limit_y_pos - limit_y_neg) * ((0:(N_line-1)) / (N_line-1)) + limit_y_neg; + + + pt = cross(repmat(l_epipole,1,N_line),[zeros(1,N_line);ones(1,N_line);-y_list]); + + +end; + + +pt = [pt(1,:) ./ pt(3,:) ; pt(2,:)./pt(3,:)]; +ptd = apply_distortion(pt,kc_right); +epipole = KK_right * [ ptd ; ones(1,N_line)]; + +epipole = epipole(1:2,:); \ No newline at end of file diff --git a/src/g_calib/compute_extrinsic.m b/src/g_calib/compute_extrinsic.m new file mode 100755 index 0000000000000000000000000000000000000000..b5260280543f29c1b7bdb67b628059f19d522d4e --- /dev/null +++ b/src/g_calib/compute_extrinsic.m @@ -0,0 +1,122 @@ +function [omckk,Tckk,Rckk,H,x,ex,JJ] = compute_extrinsic(x_kk,X_kk,fc,cc,kc,alpha_c,MaxIter,thresh_cond), + +%compute_extrinsic +% +%[omckk,Tckk,Rckk,H,x,ex] = compute_extrinsic(x_kk,X_kk,fc,cc,kc,alpha_c) +% +%Computes the extrinsic parameters attached to a 3D structure X_kk given its projection +%on the image plane x_kk and the intrinsic camera parameters fc, cc and kc. +%Works with planar and non-planar structures. +% +%INPUT: x_kk: Feature locations on the images +% X_kk: Corresponding grid coordinates +% fc: Camera focal length +% cc: Principal point coordinates +% kc: Distortion coefficients +% alpha_c: Skew coefficient +% +%OUTPUT: omckk: 3D rotation vector attached to the grid positions in space +% Tckk: 3D translation vector attached to the grid positions in space +% Rckk: 3D rotation matrices corresponding to the omc vectors +% H: Homography between points on the grid and points on the image plane (in pixel) +% This makes sense only if the planar that is used in planar. +% x: Reprojections of the points on the image plane +% ex: Reprojection error: ex = x_kk - x; +% +%Method: Computes the normalized point coordinates, then computes the 3D pose +% +%Important functions called within that program: +% +%normalize_pixel: Computes the normalize image point coordinates. +% +%pose3D: Computes the 3D pose of the structure given the normalized image projection. +% +%project_points.m: Computes the 2D image projections of a set of 3D points + + + +if nargin < 8, + thresh_cond = inf; +end; + + +if nargin < 7, + MaxIter = 20; +end; + + +if nargin < 6, + alpha_c = 0; + if nargin < 5, + kc = zeros(5,1); + if nargin < 4, + cc = zeros(2,1); + if nargin < 3, + fc = ones(2,1); + if nargin < 2, + error('Need 2D projections and 3D points (in compute_extrinsic.m)'); + return; + end; + end; + end; + end; +end; + +% Initialization: + +[omckk,Tckk,Rckk] = compute_extrinsic_init(x_kk,X_kk,fc,cc,kc,alpha_c); + +% Refinement: +[omckk,Tckk,Rckk,JJ] = compute_extrinsic_refine(omckk,Tckk,x_kk,X_kk,fc,cc,kc,alpha_c,MaxIter,thresh_cond); + + +% computation of the homography (not useful in the end) + +H = [Rckk(:,1:2) Tckk]; + +% Computes the reprojection error in pixels: + +x = project_points2(X_kk,omckk,Tckk,fc,cc,kc,alpha_c); + +ex = x_kk - x; + + +% Converts the homography in pixel units: + +KK = [fc(1) alpha_c*fc(1) cc(1);0 fc(2) cc(2); 0 0 1]; + +H = KK*H; + + + + +return; + + +% Test of compte extrinsic: + +Np = 4; +sx = 10; +sy = 10; +sz = 5; + +om = randn(3,1); +T = [0;0;100]; + +noise = 2/1000; + +XX = [sx*randn(1,Np);sy*randn(1,Np);sz*randn(1,Np)]; +xx = project_points(XX,om,T); + +xxn = xx + noise * randn(2,Np); + +[omckk,Tckk] = compute_extrinsic(xxn,XX); + +[om omckk om-omckk] +[T Tckk T-Tckk] + +figure(3); +plot(xx(1,:),xx(2,:),'r+'); +hold on; +plot(xxn(1,:),xxn(2,:),'g+'); +hold off; diff --git a/src/g_calib/compute_extrinsic_init.m b/src/g_calib/compute_extrinsic_init.m new file mode 100755 index 0000000000000000000000000000000000000000..670cf06b10b2b0829d8d953740264dc10bbe3679 --- /dev/null +++ b/src/g_calib/compute_extrinsic_init.m @@ -0,0 +1,215 @@ +function [omckk,Tckk,Rckk] = compute_extrinsic_init(x_kk,X_kk,fc,cc,kc,alpha_c), + +%compute_extrinsic +% +%[omckk,Tckk,Rckk] = compute_extrinsic_init(x_kk,X_kk,fc,cc,kc,alpha_c) +% +%Computes the extrinsic parameters attached to a 3D structure X_kk given its projection +%on the image plane x_kk and the intrinsic camera parameters fc, cc and kc. +%Works with planar and non-planar structures. +% +%INPUT: x_kk: Feature locations on the images +% X_kk: Corresponding grid coordinates +% fc: Camera focal length +% cc: Principal point coordinates +% kc: Distortion coefficients +% alpha_c: Skew coefficient +% +%OUTPUT: omckk: 3D rotation vector attached to the grid positions in space +% Tckk: 3D translation vector attached to the grid positions in space +% Rckk: 3D rotation matrices corresponding to the omc vectors +% +%Method: Computes the normalized point coordinates, then computes the 3D pose +% +%Important functions called within that program: +% +%normalize_pixel: Computes the normalize image point coordinates. +% +%pose3D: Computes the 3D pose of the structure given the normalized image projection. +% +%project_points.m: Computes the 2D image projections of a set of 3D points + + + +if nargin < 6, + alpha_c = 0; + if nargin < 5, + kc = zeros(5,1); + if nargin < 4, + cc = zeros(2,1); + if nargin < 3, + fc = ones(2,1); + if nargin < 2, + error('Need 2D projections and 3D points (in compute_extrinsic.m)'); + return; + end; + end; + end; + end; +end; + + +%keyboard; + +% Compute the normalized coordinates: + +xn = normalize_pixel(x_kk,fc,cc,kc,alpha_c); + + + +Np = size(xn,2); + +%% Check for planarity of the structure: +%keyboard; + +X_mean = mean(X_kk')'; + +Y = X_kk - (X_mean*ones(1,Np)); + +YY = Y*Y'; + +[U,S,V] = svd(YY); + +r = S(3,3)/S(2,2); + +%keyboard; + + +if (r < 1e-3)|(Np < 5), %1e-3, %1e-4, %norm(X_kk(3,:)) < eps, % Test of planarity + + %fprintf(1,'Planar structure detected: r=%f\n',r); + + % Transform the plane to bring it in the Z=0 plane: + + R_transform = V'; + + %norm(R_transform(1:2,3)) + + if norm(R_transform(1:2,3)) < 1e-6, + R_transform = eye(3); + end; + + if det(R_transform) < 0, R_transform = -R_transform; end; + + T_transform = -(R_transform)*X_mean; + + X_new = R_transform*X_kk + T_transform*ones(1,Np); + + + % Compute the planar homography: + + H = compute_homography(xn,X_new(1:2,:)); + + % De-embed the motion parameters from the homography: + + sc = mean([norm(H(:,1));norm(H(:,2))]); + + H = H/sc; + + % Extra normalization for some reasons... + %H(:,1) = H(:,1)/norm(H(:,1)); + %H(:,2) = H(:,2)/norm(H(:,2)); + + if 0, %%% Some tests for myself... the opposite sign solution leads to negative depth!!! + + % Case#1: no opposite sign: + + omckk1 = rodrigues([H(:,1:2) cross(H(:,1),H(:,2))]); + Rckk1 = rodrigues(omckk1); + Tckk1 = H(:,3); + + Hs1 = [Rckk1(:,1:2) Tckk1]; + xn1 = Hs1*[X_new(1:2,:);ones(1,Np)]; + xn1 = [xn1(1,:)./xn1(3,:) ; xn1(2,:)./xn1(3,:)]; + e1 = xn1 - xn; + + % Case#2: opposite sign: + + omckk2 = rodrigues([-H(:,1:2) cross(H(:,1),H(:,2))]); + Rckk2 = rodrigues(omckk2); + Tckk2 = -H(:,3); + + Hs2 = [Rckk2(:,1:2) Tckk2]; + xn2 = Hs2*[X_new(1:2,:);ones(1,Np)]; + xn2 = [xn2(1,:)./xn2(3,:) ; xn2(2,:)./xn2(3,:)]; + e2 = xn2 - xn; + + if 1, %norm(e1) < norm(e2), + omckk = omckk1; + Tckk = Tckk1; + Rckk = Rckk1; + else + omckk = omckk2; + Tckk = Tckk2; + Rckk = Rckk2; + end; + + else + + u1 = H(:,1); + u1 = u1 / norm(u1); + u2 = H(:,2) - dot(u1,H(:,2)) * u1; + u2 = u2 / norm(u2); + u3 = cross(u1,u2); + RRR = [u1 u2 u3]; + omckk = rodrigues(RRR); + + %omckk = rodrigues([H(:,1:2) cross(H(:,1),H(:,2))]); + Rckk = rodrigues(omckk); + Tckk = H(:,3); + + end; + + + + %If Xc = Rckk * X_new + Tckk, then Xc = Rckk * R_transform * X_kk + Tckk + T_transform + + Tckk = Tckk + Rckk* T_transform; + Rckk = Rckk * R_transform; + + omckk = rodrigues(Rckk); + Rckk = rodrigues(omckk); + + +else + + %fprintf(1,'Non planar structure detected: r=%f\n',r); + + % Computes an initial guess for extrinsic parameters (works for general 3d structure, not planar!!!): + % The DLT method is applied here!! + + J = zeros(2*Np,12); + + xX = (ones(3,1)*xn(1,:)).*X_kk; + yX = (ones(3,1)*xn(2,:)).*X_kk; + + J(1:2:end,[1 4 7]) = -X_kk'; + J(2:2:end,[2 5 8]) = X_kk'; + J(1:2:end,[3 6 9]) = xX'; + J(2:2:end,[3 6 9]) = -yX'; + J(1:2:end,12) = xn(1,:)'; + J(2:2:end,12) = -xn(2,:)'; + J(1:2:end,10) = -ones(Np,1); + J(2:2:end,11) = ones(Np,1); + + JJ = J'*J; + [U,S,V] = svd(JJ); + + RR = reshape(V(1:9,12),3,3); + + if det(RR) < 0, + V(:,12) = -V(:,12); + RR = -RR; + end; + + [Ur,Sr,Vr] = svd(RR); + + Rckk = Ur*Vr'; + + sc = norm(V(1:9,12)) / norm(Rckk(:)); + Tckk = V(10:12,12)/sc; + + omckk = rodrigues(Rckk); + Rckk = rodrigues(omckk); + +end; diff --git a/src/g_calib/compute_extrinsic_init_fisheye.m b/src/g_calib/compute_extrinsic_init_fisheye.m new file mode 100755 index 0000000000000000000000000000000000000000..97a9ea31f12971ba3685f59230e0ecb9092e7745 --- /dev/null +++ b/src/g_calib/compute_extrinsic_init_fisheye.m @@ -0,0 +1,215 @@ +function [omckk,Tckk,Rckk] = compute_extrinsic_init_fisheye(x_kk,X_kk,fc,cc,kc,alpha_c) + +%compute_extrinsic +% +%[omckk,Tckk,Rckk] = compute_extrinsic_init_fisheye(x_kk,X_kk,fc,cc,kc,alpha_c) +% +%Computes the extrinsic parameters attached to a 3D structure X_kk given its projection +%on the image plane x_kk and the intrinsic camera parameters fc, cc and kc. +%Works with planar and non-planar structures. +% +%INPUT: x_kk: Feature locations on the images +% X_kk: Corresponding grid coordinates +% fc: Camera focal length +% cc: Principal point coordinates +% kc: Distortion coefficients +% alpha_c: Skew coefficient +% +%OUTPUT: omckk: 3D rotation vector attached to the grid positions in space +% Tckk: 3D translation vector attached to the grid positions in space +% Rckk: 3D rotation matrices corresponding to the omc vectors +% +%Method: Computes the normalized point coordinates, then computes the 3D pose +% +%Important functions called within that program: +% +%normalize_pixel: Computes the normalize image point coordinates. +% +%pose3D: Computes the 3D pose of the structure given the normalized image projection. +% +%project_points.m: Computes the 2D image projections of a set of 3D points + + + +if nargin < 6, + alpha_c = 0; + if nargin < 5, + kc = zeros(5,1); + if nargin < 4, + cc = zeros(2,1); + if nargin < 3, + fc = ones(2,1); + if nargin < 2, + error('Need 2D projections and 3D points (in compute_extrinsic.m)'); + return; + end; + end; + end; + end; +end; + + +%keyboard; + +% Compute the normalized coordinates: + +xn = normalize_pixel_fisheye(x_kk,fc,cc,kc,alpha_c); + + + +Np = size(xn,2); + +%% Check for planarity of the structure: +%keyboard; + +X_mean = mean(X_kk')'; + +Y = X_kk - (X_mean*ones(1,Np)); + +YY = Y*Y'; + +[U,S,V] = svd(YY); + +r = S(3,3)/S(2,2); + +%keyboard; + + +if (r < 1e-3)|(Np < 5), %1e-3, %1e-4, %norm(X_kk(3,:)) < eps, % Test of planarity + + %fprintf(1,'Planar structure detected: r=%f\n',r); + + % Transform the plane to bring it in the Z=0 plane: + + R_transform = V'; + + %norm(R_transform(1:2,3)) + + if norm(R_transform(1:2,3)) < 1e-6, + R_transform = eye(3); + end; + + if det(R_transform) < 0, R_transform = -R_transform; end; + + T_transform = -(R_transform)*X_mean; + + X_new = R_transform*X_kk + T_transform*ones(1,Np); + + + % Compute the planar homography: + + H = compute_homography(xn,X_new(1:2,:)); + + % De-embed the motion parameters from the homography: + + sc = mean([norm(H(:,1));norm(H(:,2))]); + + H = H/sc; + + % Extra normalization for some reasons... + %H(:,1) = H(:,1)/norm(H(:,1)); + %H(:,2) = H(:,2)/norm(H(:,2)); + + if 0, %%% Some tests for myself... the opposite sign solution leads to negative depth!!! + + % Case#1: no opposite sign: + + omckk1 = rodrigues([H(:,1:2) cross(H(:,1),H(:,2))]); + Rckk1 = rodrigues(omckk1); + Tckk1 = H(:,3); + + Hs1 = [Rckk1(:,1:2) Tckk1]; + xn1 = Hs1*[X_new(1:2,:);ones(1,Np)]; + xn1 = [xn1(1,:)./xn1(3,:) ; xn1(2,:)./xn1(3,:)]; + e1 = xn1 - xn; + + % Case#2: opposite sign: + + omckk2 = rodrigues([-H(:,1:2) cross(H(:,1),H(:,2))]); + Rckk2 = rodrigues(omckk2); + Tckk2 = -H(:,3); + + Hs2 = [Rckk2(:,1:2) Tckk2]; + xn2 = Hs2*[X_new(1:2,:);ones(1,Np)]; + xn2 = [xn2(1,:)./xn2(3,:) ; xn2(2,:)./xn2(3,:)]; + e2 = xn2 - xn; + + if 1, %norm(e1) < norm(e2), + omckk = omckk1; + Tckk = Tckk1; + Rckk = Rckk1; + else + omckk = omckk2; + Tckk = Tckk2; + Rckk = Rckk2; + end; + + else + + u1 = H(:,1); + u1 = u1 / norm(u1); + u2 = H(:,2) - dot(u1,H(:,2)) * u1; + u2 = u2 / norm(u2); + u3 = cross(u1,u2); + RRR = [u1 u2 u3]; + omckk = rodrigues(RRR); + + %omckk = rodrigues([H(:,1:2) cross(H(:,1),H(:,2))]); + Rckk = rodrigues(omckk); + Tckk = H(:,3); + + end; + + + + %If Xc = Rckk * X_new + Tckk, then Xc = Rckk * R_transform * X_kk + Tckk + T_transform + + Tckk = Tckk + Rckk* T_transform; + Rckk = Rckk * R_transform; + + omckk = rodrigues(Rckk); + Rckk = rodrigues(omckk); + + +else + + %fprintf(1,'Non planar structure detected: r=%f\n',r); + + % Computes an initial guess for extrinsic parameters (works for general 3d structure, not planar!!!): + % The DLT method is applied here!! + + J = zeros(2*Np,12); + + xX = (ones(3,1)*xn(1,:)).*X_kk; + yX = (ones(3,1)*xn(2,:)).*X_kk; + + J(1:2:end,[1 4 7]) = -X_kk'; + J(2:2:end,[2 5 8]) = X_kk'; + J(1:2:end,[3 6 9]) = xX'; + J(2:2:end,[3 6 9]) = -yX'; + J(1:2:end,12) = xn(1,:)'; + J(2:2:end,12) = -xn(2,:)'; + J(1:2:end,10) = -ones(Np,1); + J(2:2:end,11) = ones(Np,1); + + JJ = J'*J; + [U,S,V] = svd(JJ); + + RR = reshape(V(1:9,12),3,3); + + if det(RR) < 0, + V(:,12) = -V(:,12); + RR = -RR; + end; + + [Ur,Sr,Vr] = svd(RR); + + Rckk = Ur*Vr'; + + sc = norm(V(1:9,12)) / norm(Rckk(:)); + Tckk = V(10:12,12)/sc; + + omckk = rodrigues(Rckk); + Rckk = rodrigues(omckk); + +end; diff --git a/src/g_calib/compute_extrinsic_refine.m b/src/g_calib/compute_extrinsic_refine.m new file mode 100755 index 0000000000000000000000000000000000000000..f000f48a8e6025447456808892bfa631026b8e1a --- /dev/null +++ b/src/g_calib/compute_extrinsic_refine.m @@ -0,0 +1,114 @@ +function [omckk,Tckk,Rckk,JJ] = compute_extrinsic_refine(omc_init,Tc_init,x_kk,X_kk,fc,cc,kc,alpha_c,MaxIter,thresh_cond), + +%compute_extrinsic +% +%[omckk,Tckk,Rckk] = compute_extrinsic_refine(omc_init,x_kk,X_kk,fc,cc,kc,alpha_c,MaxIter) +% +%Computes the extrinsic parameters attached to a 3D structure X_kk given its projection +%on the image plane x_kk and the intrinsic camera parameters fc, cc and kc. +%Works with planar and non-planar structures. +% +%INPUT: x_kk: Feature locations on the images +% X_kk: Corresponding grid coordinates +% fc: Camera focal length +% cc: Principal point coordinates +% kc: Distortion coefficients +% alpha_c: Skew coefficient +% MaxIter: Maximum number of iterations +% +%OUTPUT: omckk: 3D rotation vector attached to the grid positions in space +% Tckk: 3D translation vector attached to the grid positions in space +% Rckk: 3D rotation matrices corresponding to the omc vectors + +% +%Method: Computes the normalized point coordinates, then computes the 3D pose +% +%Important functions called within that program: +% +%normalize_pixel: Computes the normalize image point coordinates. +% +%pose3D: Computes the 3D pose of the structure given the normalized image projection. +% +%project_points.m: Computes the 2D image projections of a set of 3D points + + +if nargin < 10, + thresh_cond = inf; +end; + + +if nargin < 9, + MaxIter = 20; +end; + +if nargin < 8, + alpha_c = 0; + if nargin < 7, + kc = zeros(5,1); + if nargin < 6, + cc = zeros(2,1); + if nargin < 5, + fc = ones(2,1); + if nargin < 4, + error('Need 2D projections and 3D points (in compute_extrinsic_refine.m)'); + return; + end; + end; + end; + end; +end; + + +% Initialization: + +omckk = omc_init; +Tckk = Tc_init; + + +% Final optimization (minimize the reprojection error in pixel): +% through Gradient Descent: + +param = [omckk;Tckk]; + +change = 1; + +iter = 0; + +%keyboard; + +%fprintf(1,'Gradient descent iterations: '); + +while (change > 1e-10)&(iter < MaxIter), + + %fprintf(1,'%d...',iter+1); + + [x,dxdom,dxdT] = project_points2(X_kk,omckk,Tckk,fc,cc,kc,alpha_c); + + ex = x_kk - x; + + %keyboard; + + JJ = [dxdom dxdT]; + + if cond(JJ) > thresh_cond, + change = 0; + else + + JJ2 = JJ'*JJ; + + param_innov = inv(JJ2)*(JJ')*ex(:); + param_up = param + param_innov; + change = norm(param_innov)/norm(param_up); + param = param_up; + iter = iter + 1; + + omckk = param(1:3); + Tckk = param(4:6); + + end; + +end; + +%fprintf(1,'\n'); + +Rckk = rodrigues(omckk); diff --git a/src/g_calib/compute_extrinsic_refine2.m b/src/g_calib/compute_extrinsic_refine2.m new file mode 100755 index 0000000000000000000000000000000000000000..6be0c4962e450aaca409a3438a886a456fdccaeb --- /dev/null +++ b/src/g_calib/compute_extrinsic_refine2.m @@ -0,0 +1,119 @@ +function [omckk,Tckk,Rckk,JJ] = compute_extrinsic_refine2(omc_init,Tc_init,x_kk,X_kk,fc,cc,kc,alpha_c,MaxIter,thresh_cond), + +%compute_extrinsic +% +%[omckk,Tckk,Rckk] = compute_extrinsic_refine2(omc_init,x_kk,X_kk,fc,cc,kc,alpha_c,MaxIter) +% +%Computes the extrinsic parameters attached to a 3D structure X_kk given its projection +%on the image plane x_kk and the intrinsic camera parameters fc, cc and kc. +%Works with planar and non-planar structures. +% +%INPUT: x_kk: Feature locations on the images +% X_kk: Corresponding grid coordinates +% fc: Camera focal length +% cc: Principal point coordinates +% kc: Distortion coefficients +% alpha_c: Skew coefficient +% MaxIter: Maximum number of iterations +% +%OUTPUT: omckk: 3D rotation vector attached to the grid positions in space +% Tckk: 3D translation vector attached to the grid positions in space +% Rckk: 3D rotation matrices corresponding to the omc vectors + +% +%Method: Computes the normalized point coordinates, then computes the 3D pose +% +%Important functions called within that program: +% +%normalize_pixel: Computes the normalize image point coordinates. +% +%pose3D: Computes the 3D pose of the structure given the normalized image projection. +% +%project_points.m: Computes the 2D image projections of a set of 3D points + + +if nargin < 10, + thresh_cond = inf; +end; + + +if nargin < 9, + MaxIter = 20; +end; + +if nargin < 8, + alpha_c = 0; + if nargin < 7, + kc = zeros(5,1); + if nargin < 6, + cc = zeros(2,1); + if nargin < 5, + fc = ones(2,1); + if nargin < 4, + error('Need 2D projections and 3D points (in compute_extrinsic_refine.m)'); + return; + end; + end; + end; + end; +end; + + +% Initialization: + +omckk = omc_init; +Tckk = Tc_init; + + +% Final optimization (minimize the reprojection error in pixel): +% through Gradient Descent: + +param = [omckk;Tckk]; + +change = 1; + +iter = 0; + +%keyboard; + +%fprintf(1,'Gradient descent iterations: '); + + +% Normalize the points: +x_kk_n = normalize_pixel(x_kk,fc,cc,kc,alpha_c); + + +while (change > 1e-10)&(iter < MaxIter), + + %fprintf(1,'%d...',iter+1); + + [x,dxdom,dxdT] = project_points2(X_kk,omckk,Tckk); + + ex = x_kk_n - x; + + %keyboard; + + JJ = [dxdom dxdT]; + + if cond(JJ) > thresh_cond, + change = 0; + else + + JJ2 = JJ'*JJ; + + param_innov = inv(JJ2)*(JJ')*ex(:); + param_up = param + param_innov; + change = norm(param_innov)/norm(param_up); + param = param_up; + iter = iter + 1; + + omckk = param(1:3); + Tckk = param(4:6); + + end; + +end; + +%fprintf(1,'\n'); + +Rckk = rodrigues(omckk); diff --git a/src/g_calib/compute_extrinsic_refine_fisheye.m b/src/g_calib/compute_extrinsic_refine_fisheye.m new file mode 100755 index 0000000000000000000000000000000000000000..cecd6863706c488d411947a6328947d63d032ae9 --- /dev/null +++ b/src/g_calib/compute_extrinsic_refine_fisheye.m @@ -0,0 +1,114 @@ +function [omckk,Tckk,Rckk,JJ] = compute_extrinsic_refine_fisheye(omc_init,Tc_init,x_kk,X_kk,fc,cc,kc,alpha_c,MaxIter,thresh_cond) + +%compute_extrinsic +% +%[omckk,Tckk,Rckk] = compute_extrinsic_refine_fisheye(omc_init,x_kk,X_kk,fc,cc,kc,alpha_c,MaxIter) +% +%Computes the extrinsic parameters attached to a 3D structure X_kk given its projection +%on the image plane x_kk and the intrinsic camera parameters fc, cc and kc. +%Works with planar and non-planar structures. +% +%INPUT: x_kk: Feature locations on the images +% X_kk: Corresponding grid coordinates +% fc: Camera focal length +% cc: Principal point coordinates +% kc: Fisheye Distortion coefficients +% alpha_c: Skew coefficient +% MaxIter: Maximum number of iterations +% +%OUTPUT: omckk: 3D rotation vector attached to the grid positions in space +% Tckk: 3D translation vector attached to the grid positions in space +% Rckk: 3D rotation matrices corresponding to the omc vectors + +% +%Method: Computes the normalized point coordinates, then computes the 3D pose +% +%Important functions called within that program: +% +%normalize_pixel: Computes the normalize image point coordinates. +% +%pose3D: Computes the 3D pose of the structure given the normalized image projection. +% +%project_points.m: Computes the 2D image projections of a set of 3D points + + +if nargin < 10, + thresh_cond = inf; +end; + + +if nargin < 9, + MaxIter = 20; +end; + +if nargin < 8, + alpha_c = 0; + if nargin < 7, + kc = zeros(5,1); + if nargin < 6, + cc = zeros(2,1); + if nargin < 5, + fc = ones(2,1); + if nargin < 4, + error('Need 2D projections and 3D points (in compute_extrinsic_refine.m)'); + return; + end; + end; + end; + end; +end; + + +% Initialization: + +omckk = omc_init; +Tckk = Tc_init; + + +% Final optimization (minimize the reprojection error in pixel): +% through Gradient Descent: + +param = [omckk;Tckk]; + +change = 1; + +iter = 0; + +%keyboard; + +%fprintf(1,'Gradient descent iterations: '); + +while (change > 1e-10)&(iter < MaxIter), + + %fprintf(1,'%d...',iter+1); + + [x,dxdom,dxdT] = project_points_fisheye(X_kk,omckk,Tckk,fc,cc,kc,alpha_c); + + ex = x_kk - x; + + %keyboard; + + JJ = [dxdom dxdT]; + + if cond(JJ) > thresh_cond, + change = 0; + else + + JJ2 = JJ'*JJ; + + param_innov = inv(JJ2)*(JJ')*ex(:); + param_up = param + param_innov; + change = norm(param_innov)/norm(param_up); + param = param_up; + iter = iter + 1; + + omckk = param(1:3); + Tckk = param(4:6); + + end; + +end; + +%fprintf(1,'\n'); + +Rckk = rodrigues(omckk); diff --git a/src/g_calib/compute_homography.m b/src/g_calib/compute_homography.m new file mode 100755 index 0000000000000000000000000000000000000000..2c347180883dca65e13e3095e1f7c9dc56c38589 --- /dev/null +++ b/src/g_calib/compute_homography.m @@ -0,0 +1,175 @@ +function [H,Hnorm,inv_Hnorm] = compute_homography(m,M); + +%compute_homography +% +%[H,Hnorm,inv_Hnorm] = compute_homography(m,M) +% +%Computes the planar homography between the point coordinates on the plane (M) and the image +%point coordinates (m). +% +%INPUT: m: homogeneous coordinates in the image plane (3xN matrix) +% M: homogeneous coordinates in the plane in 3D (3xN matrix) +% +%OUTPUT: H: Homography matrix (3x3 homogeneous matrix) +% Hnorm: Normalization matrix used on the points before homography computation +% (useful for numerical stability is points in pixel coordinates) +% inv_Hnorm: The inverse of Hnorm +% +%Definition: m ~ H*M where "~" means equal up to a non zero scalar factor. +% +%Method: First computes an initial guess for the homography through quasi-linear method. +% Then, if the total number of points is larger than 4, optimize the solution by minimizing +% the reprojection error (in the least squares sense). +% +% +%Important functions called within that program: +% +%comp_distortion_oulu: Undistorts pixel coordinates. +% +%compute_homography.m: Computes the planar homography between points on the grid in 3D, and the image plane. +% +%project_points.m: Computes the 2D image projections of a set of 3D points, and also returns te Jacobian +% matrix (derivative with respect to the intrinsic and extrinsic parameters). +% This function is called within the minimization loop. + + + + +Np = size(m,2); + +if size(m,1)<3, + m = [m;ones(1,Np)]; +end; + +if size(M,1)<3, + M = [M;ones(1,Np)]; +end; + + +m = m ./ (ones(3,1)*m(3,:)); +M = M ./ (ones(3,1)*M(3,:)); + +% Prenormalization of point coordinates (very important): +% (Affine normalization) + +ax = m(1,:); +ay = m(2,:); + +mxx = mean(ax); +myy = mean(ay); +ax = ax - mxx; +ay = ay - myy; + +scxx = mean(abs(ax)); +scyy = mean(abs(ay)); + + +Hnorm = [1/scxx 0 -mxx/scxx;0 1/scyy -myy/scyy;0 0 1]; +inv_Hnorm = [scxx 0 mxx ; 0 scyy myy; 0 0 1]; + +mn = Hnorm*m; + +% Compute the homography between m and mn: + +% Build the matrix: + +L = zeros(2*Np,9); + +L(1:2:2*Np,1:3) = M'; +L(2:2:2*Np,4:6) = M'; +L(1:2:2*Np,7:9) = -((ones(3,1)*mn(1,:)).* M)'; +L(2:2:2*Np,7:9) = -((ones(3,1)*mn(2,:)).* M)'; + +if Np > 4, + L = L'*L; +end; + +[U,S,V] = svd(L); + +hh = V(:,9); +hh = hh/hh(9); + +Hrem = reshape(hh,3,3)'; +%Hrem = Hrem / Hrem(3,3); + + +% Final homography: + +H = inv_Hnorm*Hrem; + +if 0, + m2 = H*M; + m2 = [m2(1,:)./m2(3,:) ; m2(2,:)./m2(3,:)]; + merr = m(1:2,:) - m2; +end; + +%keyboard; + +%%% Homography refinement if there are more than 4 points: + +if Np > 4, + + % Final refinement: + hhv = reshape(H',9,1); + hhv = hhv(1:8); + + for iter=1:10, + + + + mrep = H * M; + + J = zeros(2*Np,8); + + MMM = (M ./ (ones(3,1)*mrep(3,:))); + + J(1:2:2*Np,1:3) = -MMM'; + J(2:2:2*Np,4:6) = -MMM'; + + mrep = mrep ./ (ones(3,1)*mrep(3,:)); + + m_err = m(1:2,:) - mrep(1:2,:); + m_err = m_err(:); + + MMM2 = (ones(3,1)*mrep(1,:)) .* MMM; + MMM3 = (ones(3,1)*mrep(2,:)) .* MMM; + + J(1:2:2*Np,7:8) = MMM2(1:2,:)'; + J(2:2:2*Np,7:8) = MMM3(1:2,:)'; + + MMM = (M ./ (ones(3,1)*mrep(3,:)))'; + + hh_innov = inv(J'*J)*J'*m_err; + + hhv_up = hhv - hh_innov; + + H_up = reshape([hhv_up;1],3,3)'; + + %norm(m_err) + %norm(hh_innov) + + hhv = hhv_up; + H = H_up; + + end; + + +end; + +if 0, + m2 = H*M; + m2 = [m2(1,:)./m2(3,:) ; m2(2,:)./m2(3,:)]; + merr = m(1:2,:) - m2; +end; + +return; + +%test of Jacobian + +mrep = H*M; +mrep = mrep ./ (ones(3,1)*mrep(3,:)); + +m_err = mrep(1:2,:) - m(1:2,:); +figure(8); +plot(m_err(1,:),m_err(2,:),'r+'); +std(m_err') diff --git a/src/g_calib/cornerfinder.m b/src/g_calib/cornerfinder.m new file mode 100755 index 0000000000000000000000000000000000000000..d51e8aa84b307631addeeb938dad239b6ecc9491 --- /dev/null +++ b/src/g_calib/cornerfinder.m @@ -0,0 +1,227 @@ +function [xc,good,bad,type] = cornerfinder(xt,I,wintx,winty,wx2,wy2); + +%[xc] = cornerfinder(xt,I); +% +%Finds the sub-pixel corners on the image I with initial guess xt +%xt and xc are 2xN matrices. The first component is the x coordinate +%(horizontal) and the second component is the y coordinate (vertical) +% +%Based on Harris corner finder method +% +%Finds corners to a precision below .1 pixel! +%Oct. 14th, 1997 - UPDATED to work with vertical and horizontal edges as well!!! +%Sept 1998 - UPDATED to handle diverged points: we keep the original points +%good is a binary vector indicating wether a feature point has been properly +%found. +% +%Add a zero zone of size wx2,wy2 +%July 15th, 1999 - Bug on the mask building... fixed + change to Gaussian mask with higher +%resolution and larger number of iterations. + + +% California Institute of Technology +% (c) Jean-Yves Bouguet -- Oct. 14th, 1997 + + + +line_feat = 1; % set to 1 to allow for extraction of line features. + +xt = xt'; +xt = fliplr(xt); + + +if nargin < 4, + winty = 5; + if nargin < 3, + wintx = 5; + end; +end; + + +if nargin < 6, + wx2 = -1; + wy2 = -1; +end; + + +%mask = ones(2*wintx+1,2*winty+1); +mask = exp(-((-wintx:wintx)'/(wintx)).^2) * exp(-((-winty:winty)/(winty)).^2); + + +% another mask: +[X,Y] = meshgrid(-winty:winty,-wintx:wintx); +mask2 = X.^2 + Y.^2; +mask2(wintx+1,winty+1) = 1; +mask2 = 1./mask2; +%mask - mask2; + + +if (wx2>0) & (wy2>0), + if ((wintx - wx2)>=2)&((winty - wy2)>=2), + mask(wintx+1-wx2:wintx+1+wx2,winty+1-wy2:winty+1+wy2)= zeros(2*wx2+1,2*wy2+1); + end; +end; + +offx = [-wintx:wintx]'*ones(1,2*winty+1); +offy = ones(2*wintx+1,1)*[-winty:winty]; + +resolution = 0.005; + +MaxIter = 10; + +[nx,ny] = size(I); +N = size(xt,1); + +xc = xt; % first guess... they don't move !!! + +type = zeros(1,N); + + +for i=1:N, + + v_extra = resolution + 1; % just larger than resolution + + compt = 0; % no iteration yet + + while (norm(v_extra) > resolution) & (compt 0, % the sub pixel + vIx = [itIx 1-itIx 0]'; % accuracy. + else + vIx = [0 1+itIx -itIx]'; + end; + if itIy > 0, + vIy = [itIy 1-itIy 0]; + else + vIy = [0 1+itIy -itIy]; + end; + + + % What if the sub image is not in? + + if (crIx-wintx-2 < 1), xmin=1; xmax = 2*wintx+5; + elseif (crIx+wintx+2 > nx), xmax = nx; xmin = nx-2*wintx-4; + else + xmin = crIx-wintx-2; xmax = crIx+wintx+2; + end; + + if (crIy-winty-2 < 1), ymin=1; ymax = 2*winty+5; + elseif (crIy+winty+2 > ny), ymax = ny; ymin = ny-2*winty-4; + else + ymin = crIy-winty-2; ymax = crIy+winty+2; + end; + + + SI = I(xmin:xmax,ymin:ymax); % The necessary neighborhood + SI = conv2(conv2(SI,vIx,'same'),vIy,'same'); + SI = SI(2:2*wintx+4,2:2*winty+4); % The subpixel interpolated neighborhood + [gy,gx] = gradient(SI); % The gradient image + gx = gx(2:2*wintx+2,2:2*winty+2); % extraction of the useful parts only + gy = gy(2:2*wintx+2,2:2*winty+2); % of the gradients + + px = cIx + offx; + py = cIy + offy; + + gxx = gx .* gx .* mask; + gyy = gy .* gy .* mask; + gxy = gx .* gy .* mask; + + + bb = [sum(sum(gxx .* px + gxy .* py)); sum(sum(gxy .* px + gyy .* py))]; + + a = sum(sum(gxx)); + b = sum(sum(gxy)); + c = sum(sum(gyy)); + + dt = a*c - b^2; + + xc2 = [c*bb(1)-b*bb(2) a*bb(2)-b*bb(1)]/dt; + + + %keyboard; + + if line_feat, + + G = [a b;b c]; + [U,S,V] = svd(G); + + %keyboard; + + % If non-invertible, then project the point onto the edge orthogonal: + + if (S(1,1)/S(2,2) > 50), + % projection operation: + xc2 = xc2 + sum((xc(i,:)-xc2).*(V(:,2)'))*V(:,2)'; + type(i) = 1; + end; + + end; + + + %keyboard; + + % G = [a b;b c]; + % [U,S,V] = svd(G); + + + % if S(1,1)/S(2,2) > 150, + % bb2 = U'*bb; + % xc2 = (V*[bb2(1)/S(1,1) ;0])'; + % else + % xc2 = [c*bb(1)-b*bb(2) a*bb(2)-b*bb(1)]/dt; + % end; + + + %if (abs(a)> 50*abs(c)), + % xc2 = [(c*bb(1)-b*bb(2))/dt xc(i,2)]; + % elseif (abs(c)> 50*abs(a)) + % xc2 = [xc(i,1) (a*bb(2)-b*bb(1))/dt]; + % else + % xc2 = [c*bb(1)-b*bb(2) a*bb(2)-b*bb(1)]/dt; + % end; + + %keyboard; + + if (isnan(xc2(1)) || isnan(xc2(2))), + xc2 = [0 0]; + end; + + v_extra = xc(i,:) - xc2; + + xc(i,:) = xc2; + + % keyboard; + + compt = compt + 1; + + end +end; + + +% check for points that diverge: + +delta_x = xc(:,1) - xt(:,1); +delta_y = xc(:,2) - xt(:,2); + +%keyboard; + + +bad = (abs(delta_x) > wintx) | (abs(delta_y) > winty); +good = ~bad; +in_bad = find(bad); + +% For the diverged points, keep the original guesses: + +xc(in_bad,:) = xt(in_bad,:); + +xc = fliplr(xc); +xc = xc'; + +bad = bad'; +good = good'; diff --git a/src/g_calib/cornerfinder2.m b/src/g_calib/cornerfinder2.m new file mode 100755 index 0000000000000000000000000000000000000000..3ad364d784edc7a1f89ed254fd63c68d7ae386d1 --- /dev/null +++ b/src/g_calib/cornerfinder2.m @@ -0,0 +1,223 @@ +function [xc,good,bad,type] = cornerfinder2(xt,I,wintx,winty,wx2,wy2); + +%[xc] = cornerfinder(xt,I); +% +%Finds the sub-pixel corners on the image I with initial guess xt +%xt and xc are 2xN matrices. The first component is the x coordinate +%(horizontal) and the second component is the y coordinate (vertical) +% +%Based on Harris corner finder method +% +%Finds corners to a precision below .1 pixel! +%Oct. 14th, 1997 - UPDATED to work with vertical and horizontal edges as well!!! +%Sept 1998 - UPDATED to handle diverged points: we keep the original points +%good is a binary vector indicating wether a feature point has been properly +%found. +% +%Add a zero zone of size wx2,wy2 +%July 15th, 1999 - Bug on the mask building... fixed + change to Gaussian mask with higher +%resolution and larger number of iterations. + + +% California Institute of Technology +% (c) Jean-Yves Bouguet -- Oct. 14th, 1997 + + + +line_feat = 1; % set to 1 to allow for extraction of line features. + +xt = xt'; +xt = fliplr(xt); + + +if nargin < 4, + winty = 5; + if nargin < 3, + wintx = 5; + end; +end; + + +if nargin < 6, + wx2 = -1; + wy2 = -1; +end; + + +%mask = ones(2*wintx+1,2*winty+1); +mask = exp(-((-wintx:wintx)'/(wintx)).^2) * exp(-((-winty:winty)/(winty)).^2); + + +% another mask: +[X,Y] = meshgrid(-winty:winty,-wintx:wintx); +mask2 = X.^2 + Y.^2; +mask2(wintx+1,winty+1) = 1; +mask2 = 1./mask2; +%mask - mask2; + + +if (wx2>0) & (wy2>0), + if ((wintx - wx2)>=2)&((winty - wy2)>=2), + mask(wintx+1-wx2:wintx+1+wx2,winty+1-wy2:winty+1+wy2)= zeros(2*wx2+1,2*wy2+1); + end; +end; + +offx = [-wintx:wintx]'*ones(1,2*winty+1); +offy = ones(2*wintx+1,1)*[-winty:winty]; + +resolution = 0.005; + +MaxIter = 10; + +[nx,ny] = size(I); +N = size(xt,1); + +xc = xt; % first guess... they don't move !!! + +type = zeros(1,N); + + +for i=1:N, + + v_extra = resolution + 1; % just larger than resolution + + compt = 0; % no iteration yet + + while (norm(v_extra) > resolution) & (compt 0, % the sub pixel + vIx = [itIx 1-itIx 0]'; % accuracy. + else + vIx = [0 1+itIx -itIx]'; + end; + if itIy > 0, + vIy = [itIy 1-itIy 0]; + else + vIy = [0 1+itIy -itIy]; + end; + + + % What if the sub image is not in? + + if (crIx-wintx-2 < 1), xmin=1; xmax = 2*wintx+5; + elseif (crIx+wintx+2 > nx), xmax = nx; xmin = nx-2*wintx-4; + else + xmin = crIx-wintx-2; xmax = crIx+wintx+2; + end; + + if (crIy-winty-2 < 1), ymin=1; ymax = 2*winty+5; + elseif (crIy+winty+2 > ny), ymax = ny; ymin = ny-2*winty-4; + else + ymin = crIy-winty-2; ymax = crIy+winty+2; + end; + + + SI = I(xmin:xmax,ymin:ymax); % The necessary neighborhood + SI = conv2(conv2(SI,vIx,'same'),vIy,'same'); + SI = SI(2:2*wintx+4,2:2*winty+4); % The subpixel interpolated neighborhood + [gy,gx] = gradient(SI); % The gradient image + gx = gx(2:2*wintx+2,2:2*winty+2); % extraction of the useful parts only + gy = gy(2:2*wintx+2,2:2*winty+2); % of the gradients + + px = cIx + offx; + py = cIy + offy; + + gxx = gx .* gx .* mask; + gyy = gy .* gy .* mask; + gxy = gx .* gy .* mask; + + + bb = [sum(sum(gxx .* px + gxy .* py)); sum(sum(gxy .* px + gyy .* py))]; + + a = sum(sum(gxx)); + b = sum(sum(gxy)); + c = sum(sum(gyy)); + + dt = a*c - b^2; + + xc2 = [c*bb(1)-b*bb(2) a*bb(2)-b*bb(1)]/dt; + + + %keyboard; + + if line_feat, + + G = [a b;b c]; + [U,S,V] = svd(G); + + %keyboard; + + % If non-invertible, then project the point onto the edge orthogonal: + + if (S(1,1)/S(2,2) > 50), + % projection operation: + xc2 = xc2 + sum((xc(i,:)-xc2).*(V(:,2)'))*V(:,2)'; + type(i) = 1; + end; + + end; + + + %keyboard; + +% G = [a b;b c]; +% [U,S,V] = svd(G); + + +% if S(1,1)/S(2,2) > 150, +% bb2 = U'*bb; +% xc2 = (V*[bb2(1)/S(1,1) ;0])'; +% else +% xc2 = [c*bb(1)-b*bb(2) a*bb(2)-b*bb(1)]/dt; +% end; + + + %if (abs(a)> 50*abs(c)), +% xc2 = [(c*bb(1)-b*bb(2))/dt xc(i,2)]; +% elseif (abs(c)> 50*abs(a)) +% xc2 = [xc(i,1) (a*bb(2)-b*bb(1))/dt]; +% else +% xc2 = [c*bb(1)-b*bb(2) a*bb(2)-b*bb(1)]/dt; +% end; + + %keyboard; + + v_extra = xc(i,:) - xc2; + + xc(i,:) = xc2; + +% keyboard; + + compt = compt + 1; + + end +end; + + +% check for points that diverge: + +delta_x = xc(:,1) - xt(:,1); +delta_y = xc(:,2) - xt(:,2); + +%keyboard; + + +bad = (abs(delta_x) > wintx) | (abs(delta_y) > winty); +good = ~bad; +in_bad = find(bad); + +% For the diverged points, keep the original guesses: + +xc(in_bad,:) = xt(in_bad,:); + +xc = fliplr(xc); +xc = xc'; + +bad = bad'; +good = good'; diff --git a/src/g_calib/cornerfinder_saddle_point.m b/src/g_calib/cornerfinder_saddle_point.m new file mode 100755 index 0000000000000000000000000000000000000000..d1d4444d75e66ecf4d67c4fca85c51d645022597 --- /dev/null +++ b/src/g_calib/cornerfinder_saddle_point.m @@ -0,0 +1,235 @@ +function [xc,good,bad,type] = cornerfinder_saddle_point(xt,I,wintx,winty,wx2,wy2); + +%[xc] = cornerfinder_saddle_point(xt,I,wintx,winty); +% +%Finds the sub-pixel corners on the image I with initial guess xt +%xt and xc are 2xN matrices. The first component is the x coordinate +%(horizontal) and the second component is the y coordinate (vertical) +% +%Based on Harris corner finder method +% +%Finds corners to a precision below .1 pixel! +%Oct. 14th, 1997 - UPDATED to work with vertical and horizontal edges as well!!! +%Sept 1998 - UPDATED to handle diverged points: we keep the original points +%good is a binary vector indicating wether a feature point has been properly +%found. +% +%Add a zero zone of size wx2,wy2 +%July 15th, 1999 - Bug on the mask building... fixed + change to Gaussian mask with higher +%resolution and larger number of iterations. + + +% California Institute of Technology +% (c) Jean-Yves Bouguet -- Oct. 14th, 1997 + + + +xt = xt'; +xt = fliplr(xt); + + +if nargin < 4, + winty = 5; + if nargin < 3, + wintx = 5; + end; +end; + + +if nargin < 6, + wx2 = -1; + wy2 = -1; +end; + + +%mask = ones(2*wintx+1,2*winty+1); +mask = exp(-((-wintx:wintx)'/(wintx)).^2) * exp(-((-winty:winty)/(winty)).^2); + + +if (wx2>0) & (wy2>0), + if ((wintx - wx2)>=2)&((winty - wy2)>=2), + mask(wintx+1-wx2:wintx+1+wx2,winty+1-wy2:winty+1+wy2)= zeros(2*wx2+1,2*wy2+1); + end; +end; + +offx = [-wintx:wintx]'*ones(1,2*winty+1); +offy = ones(2*wintx+1,1)*[-winty:winty]; + +resolution = 0.005; + +MaxIter = 10; + +[nx,ny] = size(I); +N = size(xt,1); + +xc = xt; % first guess... they don't move !!! + +type = zeros(1,N); + + +for i=1:N, + + v_extra = resolution + 1; % just larger than resolution + + compt = 0; % no iteration yet + + while (norm(v_extra) > resolution) & (compt 0, % the sub pixel + vIx = [itIx 1-itIx 0]'; % accuracy. + else + vIx = [0 1+itIx -itIx]'; + end; + if itIy > 0, + vIy = [itIy 1-itIy 0]; + else + vIy = [0 1+itIy -itIy]; + end; + + + % What if the sub image is not in? + + if (crIx-wintx-2 < 1), xmin=1; xmax = 2*wintx+5; + elseif (crIx+wintx+2 > nx), xmax = nx; xmin = nx-2*wintx-4; + else + xmin = crIx-wintx-2; xmax = crIx+wintx+2; + end; + + if (crIy-winty-2 < 1), ymin=1; ymax = 2*winty+5; + elseif (crIy+winty+2 > ny), ymax = ny; ymin = ny-2*winty-4; + else + ymin = crIy-winty-2; ymax = crIy+winty+2; + end; + + + SI = I(xmin:xmax,ymin:ymax); % The necessary neighborhood + SI = conv2(conv2(SI,vIx,'same'),vIy,'same'); + SI = SI(2:2*wintx+4,2:2*winty+4); % The subpixel interpolated neighborhood + + + px = cIx + offx; + py = cIy + offy; + + + if 1, %~saddle, + [gy,gx] = gradient(SI); % The gradient image + gx = gx(2:2*wintx+2,2:2*winty+2); % extraction of the useful parts only + gy = gy(2:2*wintx+2,2:2*winty+2); % of the gradients + gxx = gx .* gx .* mask; + gyy = gy .* gy .* mask; + gxy = gx .* gy .* mask; + + + bb = [sum(sum(gxx .* px + gxy .* py)); sum(sum(gxy .* px + gyy .* py))]; + + a = sum(sum(gxx)); + b = sum(sum(gxy)); + c = sum(sum(gyy)); + + dt = a*c - b^2; + + xc2 = [c*bb(1)-b*bb(2) a*bb(2)-b*bb(1)]/dt; + else + + SI = SI(2:2*wintx+2,2:2*winty+2); + A = repmat(mask(:),1,6) .* [px(:).^2 px(:).*py(:) py(:).^2 px(:) py(:) ones((2*wintx+1)*(2*winty+1),1)]; + param = inv(A'*A)*A'*( mask(:).*SI(:)); + xc2 = (-inv([2*param(1) param(2) ; param(2) 2*param(3) ]) * param(4:5))'; + + end; + + v_extra = xc(i,:) - xc2; + + xc(i,:) = xc2; + + + compt = compt + 1; + + end; + + + + if 1, + + cIx = xc(i,1); % + cIy = xc(i,2); % Coords. of the point + crIx = round(cIx); % on the initial image + crIy = round(cIy); % + itIx = cIx - crIx; % Coefficients + itIy = cIy - crIy; % to compute + if itIx > 0, % the sub pixel + vIx = [itIx 1-itIx 0]'; % accuracy. + else + vIx = [0 1+itIx -itIx]'; + end; + if itIy > 0, + vIy = [itIy 1-itIy 0]; + else + vIy = [0 1+itIy -itIy]; + end; + + + % What if the sub image is not in? + + if (crIx-wintx-2 < 1), xmin=1; xmax = 2*wintx+5; + elseif (crIx+wintx+2 > nx), xmax = nx; xmin = nx-2*wintx-4; + else + xmin = crIx-wintx-2; xmax = crIx+wintx+2; + end; + + if (crIy-winty-2 < 1), ymin=1; ymax = 2*winty+5; + elseif (crIy+winty+2 > ny), ymax = ny; ymin = ny-2*winty-4; + else + ymin = crIy-winty-2; ymax = crIy+winty+2; + end; + + + SI = I(xmin:xmax,ymin:ymax); % The necessary neighborhood + SI = conv2(conv2(SI,vIx,'same'),vIy,'same'); + SI = SI(2:2*wintx+4,2:2*winty+4); % The subpixel interpolated neighborhood + px = cIx + offx; + py = cIy + offy; + + SI = SI(2:2*wintx+2,2:2*winty+2); + A = repmat(mask(:),1,6) .* [px(:).^2 px(:).*py(:) py(:).^2 px(:) py(:) ones((2*wintx+1)*(2*winty+1),1)]; + param = inv(A'*A)*A'*( mask(:).*SI(:)); + xc2 = (-inv([2*param(1) param(2) ; param(2) 2*param(3) ]) * param(4:5))'; + + + v_extra = xc(i,:) - xc2; + + xc(i,:) = xc2; + end; + + + +end; + + +% check for points that diverge: + +delta_x = xc(:,1) - xt(:,1); +delta_y = xc(:,2) - xt(:,2); + +%keyboard; + + +bad = (abs(delta_x) > wintx) | (abs(delta_y) > winty); +good = ~bad; +in_bad = find(bad); + +% For the diverged points, keep the original guesses: + +xc(in_bad,:) = xt(in_bad,:); + +xc = fliplr(xc); +xc = xc'; + +bad = bad'; +good = good'; diff --git a/src/g_calib/count_squares.m b/src/g_calib/count_squares.m new file mode 100755 index 0000000000000000000000000000000000000000..54c89ee5cc9e1b1b69bceea17225f68c6570203d --- /dev/null +++ b/src/g_calib/count_squares.m @@ -0,0 +1,57 @@ +function ns = count_squares(I,x1,y1,x2,y2,win); + +[ny,nx] = size(I); + +if ((x1-win <= 0) || (x1+win >= nx) || (y1-win <= 0) || (y1+win >= ny) || ... + (x2-win <= 0) || (x2+win >= nx) || (y2-win <= 0) || (y2+win >= ny)) + ns = -1; + return; +end; + +if ((x1 - x2)^2+(y1-y2)^2) < win, + ns = -1; + return; +end; + +lambda = [y1 - y2;x2 - x1;x1*y2 - x2*y1]; +lambda = 1/sqrt(lambda(1)^2 + lambda(2)^2) * lambda; +l1 = lambda + [0;0;win]; +l2 = lambda - [0;0;win]; +dx = x2-x1; +dy = y2 - y1; + +if abs(dx) > abs(dy), + if x2 > x1, + xs = x1:x2; + else + xs = x1:-1:x2; + end; + ys = -(lambda(3) + lambda(1)*xs)/lambda(2); +else + if y2 > y1, + ys = y1:y2; + else + ys = y1:-1:y2; + end; + xs = -(lambda(3) + lambda(2)*ys)/lambda(1); +end; + +Np = length(xs); +xs_mat = ones(2*win + 1,1)*xs; +ys_mat = ones(2*win + 1,1)*ys; +win_mat = (-win:win)'*ones(1,Np); +xs_mat2 = round(xs_mat - win_mat * lambda(1)); +ys_mat2 = round(ys_mat - win_mat * lambda(2)); +ind_mat = (xs_mat2 - 1) * ny + ys_mat2; +ima_patch = zeros(2*win + 1,Np); +ima_patch(:) = I(ind_mat(:)); + +%ima2 = ima_patch(:,win+1:end-win); + +filtk = [ones(win,Np);zeros(1,Np);-ones(win,Np)]; +out_f = sum(filtk.*ima_patch); +out_f_f = conv2(out_f,[1/4 1/2 1/4],'same'); +out_f_f = out_f_f(win+1:end-win); +ns = length(find(((out_f_f(2:end)>=0)&(out_f_f(1:end-1)<0)) | ((out_f_f(2:end)<=0)&(out_f_f(1:end-1)>0))))+1; + +return; diff --git a/src/g_calib/count_squares_distorted.m b/src/g_calib/count_squares_distorted.m new file mode 100755 index 0000000000000000000000000000000000000000..6d8e2fe2b1e9f84b53806ecba0a694ca2e1615fb --- /dev/null +++ b/src/g_calib/count_squares_distorted.m @@ -0,0 +1,59 @@ +function ns = count_squares_distorted(I,x1,y1,x2,y2,win, fg, cg, kg); + +[ny,nx] = size(I); + +if ((x1-win <= 0) || (x1+win >= nx) || (y1-win <= 0) || (y1+win >= ny) || ... + (x2-win <= 0) || (x2+win >= nx) || (y2-win <= 0) || (y2+win >= ny)) + ns = -1; + return; +end; + + +if ((x1 - x2)^2+(y1-y2)^2) < win, + ns = -1; + return; +end; + +nX = round(sqrt((x1-x2)^2 + (y1-y2)^2)); +alpha_x = (0:nX)/nX; +pt1n = normalize_pixel([x1;y1]-1,fg,cg,kg,0); +pt2n = normalize_pixel([x2;y2]-1,fg,cg,kg,0); + +ptsn = repmat(pt1n,[1 nX+1]) + (pt2n - pt1n)*alpha_x; + +pts = apply_distortion2(ptsn,kg); +pts(1,:) = fg(1)*pts(1,:) + cg(1); +pts(2,:) = fg(2)*pts(2,:) + cg(2); + +% Check that the curve is within the image: +good_line = (min(pts(1,:))-win > 0) && (max(pts(1,:))+win < (nx-1)) && ... + (min(pts(2,:))-win > 0) && (max(pts(2,:))+win <(ny-1)); + +if ~good_line, + ns = -1; + return; +end; + +% Deviate the trajectory orthogonally: +lambda = [y1 - y2 ; x2 - x1]; +lambda = lambda / sqrt(sum(lambda.^2)); + +Np = size(pts,2); +xs_mat = ones(2*win + 1,1)*pts(1,:); +ys_mat = ones(2*win + 1,1)*pts(2,:); +win_mat = (-win:win)'*ones(1,Np); +xs_mat2 = round(xs_mat - win_mat * lambda(1)); +ys_mat2 = round(ys_mat - win_mat * lambda(2)); + + +ind_mat = (xs_mat2) * ny + ys_mat2 + 1; +ima_patch = zeros(2*win + 1,Np); +ima_patch(:) = I(ind_mat(:)); + +filtk = [ones(win,Np);zeros(1,Np);-ones(win,Np)]; +out_f = sum(filtk.*ima_patch); +out_f_f = conv2(out_f,[1/4 1/2 1/4],'same'); +out_f_f = out_f_f(win+1:end-win); +ns = length(find(((out_f_f(2:end)>=0)&(out_f_f(1:end-1)<0)) | ((out_f_f(2:end)<=0)&(out_f_f(1:end-1)>0))))+1; + + diff --git a/src/g_calib/count_squares_fisheye_distorted.m b/src/g_calib/count_squares_fisheye_distorted.m new file mode 100755 index 0000000000000000000000000000000000000000..aeb750bdb703a74e49bed99261d7d43fa0230947 --- /dev/null +++ b/src/g_calib/count_squares_fisheye_distorted.m @@ -0,0 +1,56 @@ +function ns = count_squares_fisheye_distorted(I,x1,y1,x2,y2,win, fg, cg, kg); + +[ny,nx] = size(I); + +if ((x1-win <= 0) || (x1+win >= nx) || (y1-win <= 0) || (y1+win >= ny) || ... + (x2-win <= 0) || (x2+win >= nx) || (y2-win <= 0) || (y2+win >= ny)) + ns = -1; + return; +end; + + +if ((x1 - x2)^2+(y1-y2)^2) < win, + ns = -1; + return; +end; + +nX = round(sqrt((x1-x2)^2 + (y1-y2)^2)); +alpha_x = (0:nX)/nX; + +pt1n = normalize_pixel_fisheye([x1;y1]-1,fg,cg,kg,0); +pt2n = normalize_pixel_fisheye([x2;y2]-1,fg,cg,kg,0); + +ptsn = repmat(pt1n,[1 nX+1]) + (pt2n - pt1n)*alpha_x; + +pts = apply_fisheye_distortion(ptsn,kg); +pts(1,:) = fg(1)*pts(1,:) + cg(1); +pts(2,:) = fg(2)*pts(2,:) + cg(2); + +% Check that the curve is within the image: +good_line = (min(pts(1,:))-win > 0) && (max(pts(1,:))+win < (nx-1)) && ... + (min(pts(2,:))-win > 0) && (max(pts(2,:))+win <(ny-1)); + +if ~good_line, + ns = -1; + return; +end; + +% Deviate the trajectory orthogonally: +lambda = [y1 - y2 ; x2 - x1]; +lambda = lambda / sqrt(sum(lambda.^2)); + +Np = size(pts,2); +xs_mat = ones(2*win + 1,1)*pts(1,:); +ys_mat = ones(2*win + 1,1)*pts(2,:); +win_mat = (-win:win)'*ones(1,Np); +xs_mat2 = round(xs_mat - win_mat * lambda(1)); +ys_mat2 = round(ys_mat - win_mat * lambda(2)); +ind_mat = (xs_mat2) * ny + ys_mat2 + 1; +ima_patch = zeros(2*win + 1,Np); +ima_patch(:) = I(ind_mat(:)); + +filtk = [ones(win,Np);zeros(1,Np);-ones(win,Np)]; +out_f = sum(filtk.*ima_patch); +out_f_f = conv2(out_f,[1/4 1/2 1/4],'same'); +out_f_f = out_f_f(win+1:end-win); +ns = length(find(((out_f_f(2:end)>=0)&(out_f_f(1:end-1)<0)) | ((out_f_f(2:end)<=0)&(out_f_f(1:end-1)>0))))+1; diff --git a/src/g_calib/dAB.m b/src/g_calib/dAB.m new file mode 100755 index 0000000000000000000000000000000000000000..277052ae80af6827d0837aae63afd8a10dd7649f --- /dev/null +++ b/src/g_calib/dAB.m @@ -0,0 +1,39 @@ +function [dABdA,dABdB] = dAB(A,B); + +% [dABdA,dABdB] = dAB(A,B); +% +% returns : dABdA and dABdB + +[p,n] = size(A); [n2,q] = size(B); + +if n2 ~= n, + error(' A and B must have equal inner dimensions'); +end; + +if issparse(A) | issparse(B) | p*q*p*n>625 , + dABdA=spalloc(p*q,p*n,p*q*n); +else + dABdA=zeros(p*q,p*n); +end; + + +for i=1:q, + for j=1:p, + ij = j + (i-1)*p; + for k=1:n, + kj = j + (k-1)*p; + dABdA(ij,kj) = B(k,i); + end; + end; +end; + + +if issparse(A) | issparse(B) | p*q*n*q>625 , + dABdB=spalloc(p*q,n*q,p*q*n); +else + dABdB=zeros(p*q,q*n); +end; + +for i=1:q + dABdB([i*p-p+1:i*p]',[i*n-n+1:i*n]) = A; +end; diff --git a/src/g_calib/data_calib.m b/src/g_calib/data_calib.m new file mode 100755 index 0000000000000000000000000000000000000000..99675a4317d9e03ef9dbc0bfb4a05f7133be49d7 --- /dev/null +++ b/src/g_calib/data_calib.m @@ -0,0 +1,108 @@ +%%% This script alets the user enter the name of the images (base name, numbering scheme,... + + +% Checks that there are some images in the directory: + +l_ras = dir('*ras'); +s_ras = size(l_ras,1); +l_bmp = dir('*bmp'); +s_bmp = size(l_bmp,1); +l_tif = dir('*tif'); +s_tif = size(l_tif,1); +l_pgm = dir('*pgm'); +s_pgm = size(l_pgm,1); +l_ppm = dir('*ppm'); +s_ppm = size(l_ppm,1); +l_jpg = dir('*jpg'); +s_jpg = size(l_jpg,1); +l_jpeg = dir('*jpeg'); +s_jpeg = size(l_jpeg,1); + +s_tot = s_ras + s_bmp + s_tif + s_pgm + s_jpg + s_ppm + s_jpeg; + +if s_tot < 1, + fprintf(1,'No image in this directory in either ras, bmp, tif, pgm, ppm or jpg format. Change directory and try again.\n'); + break; +end; + + +% IF yes, display the directory content: + +dir; + +Nima_valid = 0; + +while (Nima_valid==0), + + fprintf(1,'\n'); + calib_name = input('Basename camera calibration images (without number nor suffix): ','s'); + + format_image = '0'; + + while format_image == '0', + + format_image = input('Image format: ([]=''r''=''ras'', ''b''=''bmp'', ''t''=''tif'', ''p''=''pgm'', ''j''=''jpg'', ''m''=''ppm'') ','s'); + + if isempty(format_image), + format_image = 'ras'; + end; + + if lower(format_image(1)) == 'm', + format_image = 'ppm'; + else + if lower(format_image(1)) == 'b', + format_image = 'bmp'; + else + if lower(format_image(1)) == 't', + format_image = 'tif'; + else + if lower(format_image(1)) == 'p', + format_image = 'pgm'; + else + if lower(format_image(1)) == 'j', + format_image = 'jpg'; + else + if lower(format_image(1)) == 'r', + format_image = 'ras'; + else + if lower(format_image(1)) == 'g', + format_image = 'jpeg'; + else + disp('Invalid image format'); + format_image = '0'; % Ask for format once again + end; + end; + end; + end; + end; + end; + end; + end; + + + check_directory; + +end; + + + +%string_save = 'save calib_data n_ima type_numbering N_slots image_numbers format_image calib_name first_num'; + +%eval(string_save); + + + +if (Nima_valid~=0), + % Reading images: + + ima_read_calib; % may be launched from the toolbox itself + % Show all the calibration images: + + if ~isempty(ind_read), + + mosaic; + + end; + +end; + diff --git a/src/g_calib/data_calib_no_read.m b/src/g_calib/data_calib_no_read.m new file mode 100755 index 0000000000000000000000000000000000000000..7aa90f33650be79ed206b7d4de0feba6b314c95d --- /dev/null +++ b/src/g_calib/data_calib_no_read.m @@ -0,0 +1,99 @@ +%%% This script alets the user enter the name of the images (base name, numbering scheme,... + + +% Checks that there are some images in the directory: + +l_ras = dir('*ras'); +s_ras = size(l_ras,1); +l_bmp = dir('*bmp'); +s_bmp = size(l_bmp,1); +l_tif = dir('*tif'); +s_tif = size(l_tif,1); +l_pgm = dir('*pgm'); +s_pgm = size(l_pgm,1); +l_ppm = dir('*ppm'); +s_ppm = size(l_ppm,1); +l_jpg = dir('*jpg'); +s_jpg = size(l_jpg,1); +l_jpeg = dir('*jpeg'); +s_jpeg = size(l_jpeg,1); + +s_tot = s_ras + s_bmp + s_tif + s_pgm + s_jpg + s_ppm + s_jpeg; + +if s_tot < 1, + fprintf(1,'No image in this directory in either ras, bmp, tif, pgm, ppm or jpg format. Change directory and try again.\n'); + break; +end; + + +% IF yes, display the directory content: + +dir; + +Nima_valid = 0; + +while (Nima_valid==0), + + fprintf(1,'\n'); + calib_name = input('Basename camera calibration images (without number nor suffix): ','s'); + + format_image = '0'; + + while format_image == '0', + + format_image = input('Image format: ([]=''r''=''ras'', ''b''=''bmp'', ''t''=''tif'', ''p''=''pgm'', ''j''=''jpeg'',''g''=''jpeg'', ''m''=''ppm'') ','s'); + + if isempty(format_image), + format_image = 'ras'; + end; + + if lower(format_image(1)) == 'm', + format_image = 'ppm'; + else + if lower(format_image(1)) == 'b', + format_image = 'bmp'; + else + if lower(format_image(1)) == 't', + format_image = 'tif'; + else + if lower(format_image(1)) == 'p', + format_image = 'pgm'; + else + if lower(format_image(1)) == 'j', + format_image = 'jpg'; + else + if lower(format_image(1)) == 'r', + format_image = 'ras'; + else + if lower(format_image(1)) == 'g', + format_image = 'jpeg'; + else + disp('Invalid image format'); + format_image = '0'; % Ask for format once again + end; + end; + end; + end; + end; + end; + end; + end; + + + check_directory; + +end; + + + +if (Nima_valid~=0), + % Reading images: + + ima_read_calib_no_read; % may be launched from the toolbox itself + + + fprintf(1,'\n'); + + fprintf(1,'To display the thumbnail images of all the calibration images, you may run mosaic_no_read (may be slow)\n'); + +end; diff --git a/src/g_calib/downsample.m b/src/g_calib/downsample.m new file mode 100755 index 0000000000000000000000000000000000000000..c46086e552c7357c2dd53601512aff6d16c535d4 --- /dev/null +++ b/src/g_calib/downsample.m @@ -0,0 +1,78 @@ +function V = downsample(U) + +% Try the 5x5 filter for building the pyramid: +% Now works with color images! + +U = double(U); + +[r,c,k] = size(U); + +p = floor((r+1)/2); % row +q = floor((c+1)/2); % col + + +U2 = zeros(r+2,c+2,k); + +for i=1:k + + U2(:,:,i) = [U(:,:,i) U(:,end,i)*ones(1,2); ones(2,1)*U(end,:,i) U(end,end,i)*ones(2,2)]; + +end; + +cp = 2*(0:(p-1))+1; % row +cq = 2*(0:(q-1))+1; % col + + +r2 = length(cp); +c2 = length(cq); + +e = cq+1; +ee = cq+2; +w = cq-1; w(1) = 1; +ww = cq-2; ww(1) = 1; ww(2) = 1; +n = cp-1; n(1) = 1; +nn = cp-2; nn(1) = 1; nn(2) = 1; +s = cp+1; +ss = cp+2; + +V = zeros(r2,c2,k); + +for i = 1:k, + + V(:,:,i) = (36*U2(cp,cq,i) + 24*(U2(n,cq,i) + U2(s,cq,i) + U2(cp,e,i) + U2(cp,w,i)) + ... + 16 * (U2(n,e,i) + U2(s,e,i) + U2(n,w,i) + U2(s,w,i)) + ... + 6 * (U2(nn,cq,i) + U2(ss,cq,i) + U2(cp,ee,i) + U2(cp,ww,i)) + ... + 4 * (U2(n,ww,i) + U2(nn,w,i) + U2(n,ee,i) + U2(nn,e,i) + U2(s,ww,i) + U2(ss,w,i) + U2(s,ee,i) + U2(ss,e,i)) + ... + (U2(nn,ee,i) + U2(ss,ee,i) + U2(nn,ww,i) + U2(ss,ww,i)))/256; + +end; + +return + +% DOWNSAMPLE2 9-point subsampling (see Burt,Adelson IEEE Tcomm 31, 532) +% V = downsample2(U) + +[r,c] = size(U); + +p = floor((r+1)/2); +q = floor((c+1)/2); + + +U2 = [U U(:,end); U(end,:) U(end,end)]; + + +cq = 2*(0:(q-1))+1; +cp = 2*(0:(p-1))+1; + +%cp = 2*([1:p]'-1)+1; +%cq = 2*([1:q]-1)+1; + +e = cq+1; %e(q) = e(q)-1; +w = cq-1; w(1) = w(1)+1; +n = cp-1; n(1) = n(1)+1; +s = cp+1; %s(p) = s(p)-1; + +% Interior +V = 0.25 * U2(cp,cq) + ... + 0.125*(U2(n,cq) + U2(s,cq) + U2(cp,e) + U2(cp,w)) + ... + 0.0625*(U2(n,e) + U2(s,e) + U2(n,w) + U2(s,w)); diff --git a/src/g_calib/edgefinder.m b/src/g_calib/edgefinder.m new file mode 100755 index 0000000000000000000000000000000000000000..8777079fc3de64c7e32a252a490e00b1e8c6f53c --- /dev/null +++ b/src/g_calib/edgefinder.m @@ -0,0 +1,218 @@ +function [xc,good,bad,type] = edgefinder(xt,I,wintx,winty,wx2,wy2); + +%[xc] = cornerfinder(xt,I); +% +%Finds the sub-pixel corners on the image I with initial guess xt +%xt and xc are 2xN matrices. The first component is the x coordinate +%(horizontal) and the second component is the y coordinate (vertical) +% +%Based on Harris corner finder method +% +%Finds corners to a precision below .1 pixel! +%Oct. 14th, 1997 - UPDATED to work with vertical and horizontal edges as well!!! +%Sept 1998 - UPDATED to handle diverged points: we keep the original points +%good is a binary vector indicating wether a feature point has been properly +%found. +% +%Add a zero zone of size wx2,wy2 +%July 15th, 1999 - Bug on the mask building... fixed + change to Gaussian mask with higher +%resolution and larger number of iterations. + + +% California Institute of Technology +% (c) Jean-Yves Bouguet -- Oct. 14th, 1997 + + +% WARNING!!! This function has not been fully tested!!! + + + +line_feat = 1; % set to 1 to allow for extraction of line features. + +xt = xt'; +xt = fliplr(xt); + + +if nargin < 4, + winty = 5; + if nargin < 3, + wintx = 5; + end; +end; + + +if nargin < 6, + wx2 = -1; + wy2 = -1; +end; + + +%mask = ones(2*wintx+1,2*winty+1); +mask = exp(-((-wintx:wintx)'/(wintx)).^2) * exp(-((-winty:winty)/(winty)).^2); + + +if (wx2>0) & (wy2>0), + if ((wintx - wx2)>=2)&((winty - wy2)>=2), + mask(wintx+1-wx2:wintx+1+wx2,winty+1-wy2:winty+1+wy2)= zeros(2*wx2+1,2*wy2+1); + end; +end; + +offx = [-wintx:wintx]'*ones(1,2*winty+1); +offy = ones(2*wintx+1,1)*[-winty:winty]; + +resolution = 0.005; + +MaxIter = 10; + +[nx,ny] = size(I); +N = size(xt,1); + +xc = xt; % first guess... they don't move !!! + +type = zeros(1,N); + + +for i=1:N, + + v_extra = resolution + 1; % just larger than resolution + + compt = 0; % no iteration yet + + while (norm(v_extra) > resolution) & (compt 0, % the sub pixel + vIx = [itIx 1-itIx 0]'; % accuracy. + else + vIx = [0 1+itIx -itIx]'; + end; + if itIy > 0, + vIy = [itIy 1-itIy 0]; + else + vIy = [0 1+itIy -itIy]; + end; + + + % What if the sub image is not in? + + if (crIx-wintx-2 < 1), xmin=1; xmax = 2*wintx+5; + elseif (crIx+wintx+2 > nx), xmax = nx; xmin = nx-2*wintx-4; + else + xmin = crIx-wintx-2; xmax = crIx+wintx+2; + end; + + if (crIy-winty-2 < 1), ymin=1; ymax = 2*winty+5; + elseif (crIy+winty+2 > ny), ymax = ny; ymin = ny-2*winty-4; + else + ymin = crIy-winty-2; ymax = crIy+winty+2; + end; + + + SI = I(xmin:xmax,ymin:ymax); % The necessary neighborhood + SI = conv2(conv2(SI,vIx,'same'),vIy,'same'); + SI = SI(2:2*wintx+4,2:2*winty+4); % The subpixel interpolated neighborhood + [gy,gx] = gradient(SI); % The gradient image + gx = gx(2:2*wintx+2,2:2*winty+2); % extraction of the useful parts only + gy = gy(2:2*wintx+2,2:2*winty+2); % of the gradients + + px = cIx + offx; + py = cIy + offy; + + gxx = gx .* gx .* mask; + gyy = gy .* gy .* mask; + gxy = gx .* gy .* mask; + + + bb = [sum(sum(gxx .* px + gxy .* py)); sum(sum(gxy .* px + gyy .* py))]; + + a = sum(sum(gxx)); + b = sum(sum(gxy)); + c = sum(sum(gyy)); + + dt = a*c - b^2; + + xc2 = [c*bb(1)-b*bb(2) a*bb(2)-b*bb(1)]/dt; + + + %keyboard; + + %if line_feat, + + G = [a b;b c]; + [U,S,V] = svd(G); + + %keyboard; + + % If non-invertible, then project the point onto the edge orthogonal: + + %if (S(1,1)/S(2,2) > 50), + % projection operation: + xc2 = xc2 + sum((xc(i,:)-xc2).*(V(:,2)'))*V(:,2)'; + type(i) = 1; + %end; + + %end; + + + %keyboard; + +% G = [a b;b c]; +% [U,S,V] = svd(G); + + +% if S(1,1)/S(2,2) > 150, +% bb2 = U'*bb; +% xc2 = (V*[bb2(1)/S(1,1) ;0])'; +% else +% xc2 = [c*bb(1)-b*bb(2) a*bb(2)-b*bb(1)]/dt; +% end; + + + %if (abs(a)> 50*abs(c)), +% xc2 = [(c*bb(1)-b*bb(2))/dt xc(i,2)]; +% elseif (abs(c)> 50*abs(a)) +% xc2 = [xc(i,1) (a*bb(2)-b*bb(1))/dt]; +% else +% xc2 = [c*bb(1)-b*bb(2) a*bb(2)-b*bb(1)]/dt; +% end; + + %keyboard; + + v_extra = xc(i,:) - xc2; + + xc(i,:) = xc2; + +% keyboard; + + compt = compt + 1; + + end +end; + + +% check for points that diverge: + +delta_x = xc(:,1) - xt(:,1); +delta_y = xc(:,2) - xt(:,2); + +%keyboard; + + +bad = (abs(delta_x) > wintx) | (abs(delta_y) > winty); +good = ~bad; +in_bad = find(bad); + +% For the diverged points, keep the original guesses: + +xc(in_bad,:) = xt(in_bad,:); + +xc = fliplr(xc); +xc = xc'; + +bad = bad'; +good = good'; diff --git a/src/g_calib/eliminate_boundary.m b/src/g_calib/eliminate_boundary.m new file mode 100755 index 0000000000000000000000000000000000000000..2dcea7e23efb9042c4a949a658b063e28babfb6a --- /dev/null +++ b/src/g_calib/eliminate_boundary.m @@ -0,0 +1,9 @@ +function Im2 = eliminate_boundary(Im); + + +[nny,nnx] = size(Im); + +Im2 = zeros(nny,nnx); + +Im2(2:end-1,2:end-1) = (Im(2:end-1,2:end-1) & Im(1:end-2,2:end-1) & Im(1:end-2,1:end-2) & Im(1:end-2,3:end) & ... + Im(3:end,2:end-1) & Im(3:end,1:end-2) & Im(3:end,3:end) & Im(2:end-1,1:end-2) & Im(2:end-1,3:end)); diff --git a/src/g_calib/error_analysis.m b/src/g_calib/error_analysis.m new file mode 100755 index 0000000000000000000000000000000000000000..85feac5e5f591e8856f331ed18d62affeb694b1b --- /dev/null +++ b/src/g_calib/error_analysis.m @@ -0,0 +1,182 @@ +%%% ERROR_ANALYSIS +%%% This simulation helps coputing the acturacies of calibration +%%% Run it after the main calibration + + + +N_runs = 200; + +%N_ima_active = 4; + +saving = 1; + +if 1, %~exist('fc_list'), % initialization + + % Initialization: + + load Calib_Results; + check_active_images; + + fc_list = []; + cc_list = []; + kc_list = []; + active_images_list = []; + + + for kk=1:n_ima, + + eval(['omc_list_' num2str(kk) ' = [];']); + eval(['Tc_list_' num2str(kk) ' = [];']); + + end; + + %sx = median(abs(ex(1,:)))*1.4836; + %sy = median(abs(ex(2,:)))*1.4836; + + sx = std(ex(1,:)); + sy = std(ex(2,:)); + + % Saving the feature locations: + + for kk = 1:n_ima, + + eval(['x_save_' num2str(kk) ' = x_' num2str(kk) ';']); + eval(['y_save_' num2str(kk) ' = y_' num2str(kk) ';']); + + end; + + active_images_save = active_images; + ind_active_save = ind_active; + + fc_save = fc; + cc_save = cc; + kc_save = kc; + KK_save = KK; + + +end; + + + + +%%% The main loop: + + +for ntrial = 1:N_runs, + + fprintf(1,'\nRun number: %d\n',ntrial); + fprintf(1, '----------\n'); + + for kk = 1:n_ima, + + eval(['y_kk = y_save_' num2str(kk) ';']) + + if active_images(kk) & ~isnan(y_kk(1,1)), + + Nkk = size(y_kk,2); + + x_kk_new = y_kk + [sx * randn(1,Nkk);sy*randn(1,Nkk)]; + + eval(['x_' num2str(kk) ' = x_kk_new;']); + + end; + + end; + + N_active = length(ind_active_save); + junk = randn(1,N_active); + [junk,junk2] = sort(junk); + + active_images = zeros(1,n_ima); + active_images(ind_active_save(junk2(1:N_ima_active))) = ones(1,N_ima_active); + + fc = fc_save; + cc = cc_save; + kc = kc_save; + KK = KK_save; + + go_calib_optim; + + fc_list = [fc_list fc]; + cc_list = [cc_list cc]; + kc_list = [kc_list kc]; + active_images_list = [active_images_list active_images']; + + for kk=1:n_ima, + + eval(['omc_list_' num2str(kk) ' = [ omc_list_' num2str(kk) ' omc_' num2str(kk) ' ];']); + eval(['Tc_list_' num2str(kk) ' = [ Tc_list_' num2str(kk) ' Tc_' num2str(kk) ' ];']); + + end; + +end; + + + + +if 0, + +% Restoring the feature locations: + +for kk = 1:n_ima, + + eval(['x_' num2str(kk) ' = x_save_' num2str(kk) ';']); + +end; + +fprintf(1,'\nFinal run (with the real data)\n'); +fprintf(1, '------------------------------\n'); + +active_images = active_images_save; +ind_active = ind_active_save; + +go_calib_optim; + +fc_list = [fc_list fc]; +cc_list = [cc_list cc]; +kc_list = [kc_list kc]; +active_images_list = [active_images_list active_images']; + +for kk=1:n_ima, + + eval(['omc_list_' num2str(kk) ' = [ omc_list_' num2str(kk) ' omc_' num2str(kk) ' ];']); + eval(['Tc_list_' num2str(kk) ' = [ Tc_list_' num2str(kk) ' Tc_' num2str(kk) ' ];']); + +end; + +end; + + + + + +if saving, + +disp(['Save Calibration accuracy results under Calib_Accuracies_' num2str(N_ima_active) '.mat']); + +string_save = ['save Calib_Accuracies_' num2str(N_ima_active) ' active_images n_ima N_ima_active N_runs active_images_list fc cc kc fc_list cc_list kc_list']; + +for kk = 1:n_ima, + string_save = [string_save ' Tc_list_' num2str(kk) ' omc_list_' num2str(kk) ' Tc_' num2str(kk) ' omc_' num2str(kk) ]; +end; + +eval(string_save); + +end; + + +return; + +std(fc_list') + +std(cc_list') + +std(kc_list') + +for kk = 1:n_ima, + + eval(['std(Tc_list_' num2str(kk) ''')']) + +end; + + diff --git a/src/g_calib/error_cam_proj.m b/src/g_calib/error_cam_proj.m new file mode 100755 index 0000000000000000000000000000000000000000..801f8980ac33f07c8bd42613d6634735c407e66b --- /dev/null +++ b/src/g_calib/error_cam_proj.m @@ -0,0 +1,61 @@ +function e_global = error_cam_proj(param); + +global n_ima x_1 X_1 xproj_1 x_proj_1 x_2 X_2 xproj_2 x_proj_2 x_3 X_3 xproj_3 x_proj_3 x_4 X_4 xproj_4 x_proj_4 x_5 X_5 xproj_5 x_proj_5 x_6 X_6 xproj_6 x_proj_6 x_7 X_7 xproj_7 x_proj_7 x_8 X_8 xproj_8 x_proj_8 x_9 X_9 xproj_9 x_proj_9 x_10 X_10 xproj_10 x_proj_10 x_11 X_11 xproj_11 x_proj_11 x_12 X_12 xproj_12 x_proj_12 x_13 X_13 xproj_13 x_proj_13 x_14 X_14 xproj_14 x_proj_14 x_15 X_15 xproj_15 x_proj_15 x_16 X_16 xproj_16 x_proj_16 x_17 X_17 xproj_17 x_proj_17 x_18 X_18 xproj_18 x_proj_18 x_19 X_19 xproj_19 x_proj_19 x_20 X_20 xproj_20 x_proj_20 x_21 X_21 xproj_21 x_proj_21 x_22 X_22 xproj_22 x_proj_22 x_23 X_23 xproj_23 x_proj_23 x_24 X_24 xproj_24 x_proj_24 x_25 X_25 xproj_25 x_proj_25 x_26 X_26 xproj_26 x_proj_26 x_27 X_27 xproj_27 x_proj_27 x_28 X_28 xproj_28 x_proj_28 x_29 X_29 xproj_29 x_proj_29 x_30 X_30 xproj_30 x_proj_30 + +% Computation of the errors: + +fc = param(1:2); +cc = param(3:4); +alpha_c = param(5); +kc = param(6:10); + +e_cam = []; + +for kk = 1:n_ima, + omckk = param(11+(kk-1)*6:11+(kk-1)*6+2); + Tckk = param(11+(kk-1)*6+3:11+(kk-1)*6+3+2); + + eval(['Xkk = X_' num2str(kk) ';']); + eval(['xkk = x_' num2str(kk) ';']); + + ekk = xkk - project_points2(Xkk,omckk,Tckk,fc,cc,kc,alpha_c); + + Rckk = rodrigues(omckk); + eval(['omc_' num2str(kk) '= omckk;']); + eval(['Tc_' num2str(kk) '= Tckk;']); + eval(['Rc_' num2str(kk) '= Rckk;']); + + e_cam = [e_cam ekk]; + +end; + +X_proj = []; +x_proj = []; + +for kk = 1:n_ima, + eval(['xproj = xproj_' num2str(kk) ';']); + xprojn = normalize_pixel(xproj,fc,cc,kc,alpha_c); + eval(['Rc = Rc_' num2str(kk) ';']); + eval(['Tc = Tc_' num2str(kk) ';']); + Np_proj = size(xproj,2); + Zc = ((Rc(:,3)'*Tc) * (1./(Rc(:,3)' * [xprojn; ones(1,Np_proj)]))); + Xcp = (ones(3,1)*Zc) .* [xprojn; ones(1,Np_proj)]; % % in the camera frame + eval(['X_proj_' num2str(kk) ' = Xcp;']); % coordinates of the points in the + eval(['X_proj = [X_proj X_proj_' num2str(kk) '];']); + eval(['x_proj = [x_proj x_proj_' num2str(kk) '];']); +end; + +fp = param((1:2)+n_ima * 6 + 10); +cp = param((3:4)+n_ima * 6 + 10); +alpha_p = param((5)+n_ima * 6 + 10); +kp = param((6:10)+n_ima * 6 + 10); + +om = param(10+n_ima*6+10+1:10+n_ima*6+10+1+2); +T = param(10+n_ima*6+10+1+2+1:10+n_ima*6+10+1+2+1+2); + + +e_proj = x_proj - project_points2(X_proj,om,T,fp,cp,kp,alpha_p); + + +e_global = [e_cam e_proj]; + diff --git a/src/g_calib/error_cam_proj2.m b/src/g_calib/error_cam_proj2.m new file mode 100755 index 0000000000000000000000000000000000000000..5671f1c83c280ae85e7630a80d0b69a21e99b7a3 --- /dev/null +++ b/src/g_calib/error_cam_proj2.m @@ -0,0 +1,63 @@ +function e_global = error_cam_proj(param); + +global n_ima x_1 X_1 xproj_1 x_proj_1 x_2 X_2 xproj_2 x_proj_2 x_3 X_3 xproj_3 x_proj_3 x_4 X_4 xproj_4 x_proj_4 x_5 X_5 xproj_5 x_proj_5 x_6 X_6 xproj_6 x_proj_6 x_7 X_7 xproj_7 x_proj_7 x_8 X_8 xproj_8 x_proj_8 x_9 X_9 xproj_9 x_proj_9 x_10 X_10 xproj_10 x_proj_10 x_11 X_11 xproj_11 x_proj_11 x_12 X_12 xproj_12 x_proj_12 x_13 X_13 xproj_13 x_proj_13 x_14 X_14 xproj_14 x_proj_14 x_15 X_15 xproj_15 x_proj_15 x_16 X_16 xproj_16 x_proj_16 x_17 X_17 xproj_17 x_proj_17 x_18 X_18 xproj_18 x_proj_18 x_19 X_19 xproj_19 x_proj_19 x_20 X_20 xproj_20 x_proj_20 x_21 X_21 xproj_21 x_proj_21 x_22 X_22 xproj_22 x_proj_22 x_23 X_23 xproj_23 x_proj_23 x_24 X_24 xproj_24 x_proj_24 x_25 X_25 xproj_25 x_proj_25 x_26 X_26 xproj_26 x_proj_26 x_27 X_27 xproj_27 x_proj_27 x_28 X_28 xproj_28 x_proj_28 x_29 X_29 xproj_29 x_proj_29 x_30 X_30 xproj_30 x_proj_30 + +% This is the same model, but with a simpler distortion model (no 6th order) + +% Computation of the errors: + +fc = param(1:2); +cc = param(3:4); +alpha_c = param(5); +kc = [param(6:9);0]; + +e_cam = []; + +for kk = 1:n_ima, + omckk = param(11+(kk-1)*6-1:11+(kk-1)*6+2-1); + Tckk = param(11+(kk-1)*6+3-1:11+(kk-1)*6+3+2-1); + + eval(['Xkk = X_' num2str(kk) ';']); + eval(['xkk = x_' num2str(kk) ';']); + + ekk = xkk - project_points2(Xkk,omckk,Tckk,fc,cc,kc,alpha_c); + + Rckk = rodrigues(omckk); + eval(['omc_' num2str(kk) '= omckk;']); + eval(['Tc_' num2str(kk) '= Tckk;']); + eval(['Rc_' num2str(kk) '= Rckk;']); + + e_cam = [e_cam ekk]; + +end; + +X_proj = []; +x_proj = []; + +for kk = 1:n_ima, + eval(['xproj = xproj_' num2str(kk) ';']); + xprojn = normalize_pixel(xproj,fc,cc,kc,alpha_c); + eval(['Rc = Rc_' num2str(kk) ';']); + eval(['Tc = Tc_' num2str(kk) ';']); + Np_proj = size(xproj,2); + Zc = ((Rc(:,3)'*Tc) * (1./(Rc(:,3)' * [xprojn; ones(1,Np_proj)]))); + Xcp = (ones(3,1)*Zc) .* [xprojn; ones(1,Np_proj)]; % % in the camera frame + eval(['X_proj_' num2str(kk) ' = Xcp;']); % coordinates of the points in the + eval(['X_proj = [X_proj X_proj_' num2str(kk) '];']); + eval(['x_proj = [x_proj x_proj_' num2str(kk) '];']); +end; + +fp = param((1:2)+n_ima * 6 + 10-1); +cp = param((3:4)+n_ima * 6 + 10-1); +alpha_p = param((5)+n_ima * 6 + 10-1); +kp = [param((6-1:10-2)+n_ima * 6 + 10);0]; + +om = param(10+n_ima*6+10+1-2:10+n_ima*6+10+1+2-2); +T = param(10+n_ima*6+10+1+2+1-2:10+n_ima*6+10+1+2+1+2-2); + + +e_proj = x_proj - project_points2(X_proj,om,T,fp,cp,kp,alpha_p); + + +e_global = [e_cam e_proj]; + diff --git a/src/g_calib/error_cam_proj3.m b/src/g_calib/error_cam_proj3.m new file mode 100755 index 0000000000000000000000000000000000000000..648a725d994636776e4f4182aff7a87d52e82b70 --- /dev/null +++ b/src/g_calib/error_cam_proj3.m @@ -0,0 +1,63 @@ +function e_global = error_cam_proj(param); + +global n_ima x_1 X_1 xproj_1 x_proj_1 x_2 X_2 xproj_2 x_proj_2 x_3 X_3 xproj_3 x_proj_3 x_4 X_4 xproj_4 x_proj_4 x_5 X_5 xproj_5 x_proj_5 x_6 X_6 xproj_6 x_proj_6 x_7 X_7 xproj_7 x_proj_7 x_8 X_8 xproj_8 x_proj_8 x_9 X_9 xproj_9 x_proj_9 x_10 X_10 xproj_10 x_proj_10 x_11 X_11 xproj_11 x_proj_11 x_12 X_12 xproj_12 x_proj_12 x_13 X_13 xproj_13 x_proj_13 x_14 X_14 xproj_14 x_proj_14 x_15 X_15 xproj_15 x_proj_15 x_16 X_16 xproj_16 x_proj_16 x_17 X_17 xproj_17 x_proj_17 x_18 X_18 xproj_18 x_proj_18 x_19 X_19 xproj_19 x_proj_19 x_20 X_20 xproj_20 x_proj_20 x_21 X_21 xproj_21 x_proj_21 x_22 X_22 xproj_22 x_proj_22 x_23 X_23 xproj_23 x_proj_23 x_24 X_24 xproj_24 x_proj_24 x_25 X_25 xproj_25 x_proj_25 x_26 X_26 xproj_26 x_proj_26 x_27 X_27 xproj_27 x_proj_27 x_28 X_28 xproj_28 x_proj_28 x_29 X_29 xproj_29 x_proj_29 x_30 X_30 xproj_30 x_proj_30 + +% This is the same model, but with a simpler distortion model (no 6th order) + +% Computation of the errors: + +fc = param(1:2); +cc = param(3:4); +alpha_c = 0; +kc = [param(5);zeros(4,1)]; + +e_cam = []; + +for kk = 1:n_ima, + omckk = param(kk*6:kk*6+2); + Tckk = param(kk*6+3:kk*6+5); + + eval(['Xkk = X_' num2str(kk) ';']); + eval(['xkk = x_' num2str(kk) ';']); + + ekk = xkk - project_points2(Xkk,omckk,Tckk,fc,cc,kc,alpha_c); + + Rckk = rodrigues(omckk); + eval(['omc_' num2str(kk) '= omckk;']); + eval(['Tc_' num2str(kk) '= Tckk;']); + eval(['Rc_' num2str(kk) '= Rckk;']); + + e_cam = [e_cam ekk]; + +end; + +X_proj = []; +x_proj = []; + +for kk = 1:n_ima, + eval(['xproj = xproj_' num2str(kk) ';']); + xprojn = normalize_pixel(xproj,fc,cc,kc,alpha_c); + eval(['Rc = Rc_' num2str(kk) ';']); + eval(['Tc = Tc_' num2str(kk) ';']); + Np_proj = size(xproj,2); + Zc = ((Rc(:,3)'*Tc) * (1./(Rc(:,3)' * [xprojn; ones(1,Np_proj)]))); + Xcp = (ones(3,1)*Zc) .* [xprojn; ones(1,Np_proj)]; % % in the camera frame + eval(['X_proj_' num2str(kk) ' = Xcp;']); % coordinates of the points in the + eval(['X_proj = [X_proj X_proj_' num2str(kk) '];']); + eval(['x_proj = [x_proj x_proj_' num2str(kk) '];']); +end; + +fp = param((1:2)+n_ima * 6 + 5); +cp = param((3:4)+n_ima * 6 + 5); +alpha_p = 0; +kp = [param((5)+n_ima * 6 + 5);zeros(4,1)]; + +om = param((6:8)+n_ima * 6 + 5); +T = param((9:11)+n_ima * 6 + 5); + + +e_proj = x_proj - project_points2(X_proj,om,T,fp,cp,kp,alpha_p); + + +e_global = [e_cam e_proj]; + diff --git a/src/g_calib/error_depth.m b/src/g_calib/error_depth.m new file mode 100755 index 0000000000000000000000000000000000000000..fa19ba13db47f281b452d32f4b9fdeb6936120e8 --- /dev/null +++ b/src/g_calib/error_depth.m @@ -0,0 +1,38 @@ +function err_shape = error_depth(param_dist,xcn,xpn,R,T,X_shape,ind); + + + +X_new = depth_compute(xcn,xpn,[param_dist],R,T); + + +N_pt_calib = size(xcn,2); + +% UnNormalized shape extraction: + +X_shape2 = X_new; +X_shape2 = X_shape2 - (X_shape2(:,1)*ones(1,N_pt_calib)); + +% map the second vector at [1;0;0]: + +omu = -cross([1;0;0],X_shape2(:,2)); +omu = acos((dot([1;0;0],X_shape2(:,2)))/norm(X_shape2(:,2)))*(omu / norm(omu)); +Ru = rodrigues(omu); + +X_shape2 = Ru* X_shape2; + +omu2 = -cross([0;1;0],[0;X_shape2(2:3,ind)]); +omu2 = acos((dot([0;1;0],[0;X_shape2(2:3,ind)]))/norm([0;X_shape2(2:3,ind)]))*(omu2 / norm(omu2)); +Ru2 = rodrigues(omu2); + +X_shape2 = Ru2* X_shape2; + + +% Error: + +err_shape = X_shape2(:,2:end) - X_shape(:,2:end); + +err_shape = err_shape(:); + + + +%err_depth = Z_new - Z_ref; diff --git a/src/g_calib/error_depth_list.m b/src/g_calib/error_depth_list.m new file mode 100755 index 0000000000000000000000000000000000000000..53ee10f19b48b1178221c6f9c573cc96807cb427 --- /dev/null +++ b/src/g_calib/error_depth_list.m @@ -0,0 +1,59 @@ +function err_total = error_depth_list(param_dist,xcn_list,xpn_list,R,T,X_shape_list,ind_list); + + +N_view = length(ind_list); + +err_total = []; + +N_pts = zeros(1,N_view); + +for kk = 1:N_view, + + xcn = xcn_list{kk}; + xpn = xpn_list{kk}; + ind = ind_list{kk}; + + xpn = xpn([1 3],:); + + X_shape = X_shape_list{kk}; + + +X_new = depth_compute(xcn,xpn,[param_dist],R,T); + + +N_pt_calib = size(xcn,2); + +% UnNormalized shape extraction: + +X_shape2 = X_new; +X_shape2 = X_shape2 - (X_shape2(:,1)*ones(1,N_pt_calib)); + +% map the second vector at [1;0;0]: + +omu = -cross([1;0;0],X_shape2(:,2)); +omu = acos((dot([1;0;0],X_shape2(:,2)))/norm(X_shape2(:,2)))*(omu / norm(omu)); +Ru = rodrigues(omu); + +X_shape2 = Ru* X_shape2; + +omu2 = -cross([0;1;0],[0;X_shape2(2:3,ind)]); +omu2 = acos((dot([0;1;0],[0;X_shape2(2:3,ind)]))/norm([0;X_shape2(2:3,ind)]))*(omu2 / norm(omu2)); +Ru2 = rodrigues(omu2); + +X_shape2 = Ru2* X_shape2; + + +% Error: + +err_shape = X_shape2(:,2:end) - X_shape(:,2:end); + +err_shape = err_shape(:); + +N_pts(kk) = N_pt_calib; + +err_total = [ err_total ; err_shape ]; + +end; + + +%err_depth = Z_new - Z_ref; diff --git a/src/g_calib/export_calib_data.m b/src/g_calib/export_calib_data.m new file mode 100755 index 0000000000000000000000000000000000000000..c66ea1a1a1726f85a2905d88cb4796381c54dc1a --- /dev/null +++ b/src/g_calib/export_calib_data.m @@ -0,0 +1,104 @@ +%% Export calibration data (corners + 3D coordinates) to +%% text files (in Willson-Heikkila's format or Zhang's format) + +%% Thanks to Michael Goesele (from the Max-Planck-Institut) for the original suggestion +%% of adding this export function to the toolbox. + + +if ~exist('n_ima'), + fprintf(1,'ERROR: No calibration data to export\n'); + +else + + if n_ima == 0, + fprintf(1,'ERROR: No calibration data to export\n'); + return; + end; + + check_active_images; + + check_extracted_images; + + check_active_images; + + fprintf(1,'Tool that exports calibration data to Willson-Heikkila or Zhang formats\n'); + + qformat = -1; + + while (qformat ~=0)&(qformat ~=1), + + fprintf(1,'Two possible formats of export: 0=Willson and Heikkila, 1=Zhang\n') + qformat = input('Format of export (enter 0 or 1): '); + + if isempty(qformat) + qformat = -1; + end; + + if (qformat ~=0)&(qformat ~=1), + + fprintf(1,'Invalid entry. Try again.\n') + + end; + + end; + + if qformat == 0, + + fprintf(1,'\nExport of calibration data to text files (Willson and Heikkila''s format)\n'); + outputfile = input('File basename: ','s'); + + for kk = ind_active, + + eval(['X_kk = X_' num2str(kk) ';']); + eval(['x_kk = x_' num2str(kk) ';']); + + Xx = [X_kk ; x_kk]'; + + file_name = [outputfile num2str(kk)]; + + disp(['Exporting calibration data (3D world + 2D image coordinates) of image ' num2str(kk) ' to file ' file_name '...']); + + eval(['save ' file_name ' Xx -ASCII']); + + end; + + else + + fprintf(1,'\nExport of calibration data to text files (Zhang''s format)\n'); + modelfile = input('File basename for the 3D world coordinates: ','s'); + datafile = input('File basename for the 2D image coordinates: ','s'); + + for kk = ind_active, + + eval(['X_kk = X_' num2str(kk) ';']); + eval(['x_kk = x_' num2str(kk) ';']); + + if ~exist(['n_sq_x_' num2str(kk)]), + n_sq_x = 1; + n_sq_y = size(X_kk,2); + else + eval(['n_sq_x = n_sq_x_' num2str(kk) ';']); + eval(['n_sq_y = n_sq_y_' num2str(kk) ';']); + end; + + X = reshape(X_kk(1,:)',n_sq_x+1,n_sq_y+1)'; + Y = reshape(X_kk(2,:)',n_sq_x+1,n_sq_y+1)'; + XY = reshape([X;Y],n_sq_y+1,2*(n_sq_x+1)); + + x = reshape(x_kk(1,:)',n_sq_x+1,n_sq_y+1)'; + y = reshape(x_kk(2,:)',n_sq_x+1,n_sq_y+1)'; + xy = reshape([x;y],n_sq_y+1,2*(n_sq_x+1)); + + disp(['Exporting calibration data of image ' num2str(kk) ' to files ' modelfile num2str(kk) '.txt and ' datafile num2str(kk) '.txt...']); + + eval(['save ' modelfile num2str(kk) '.txt XY -ASCII']); + eval(['save ' datafile num2str(kk) '.txt xy -ASCII']); + + end; + + +end; + +fprintf(1,'done\n'); + +end; diff --git a/src/g_calib/ext_calib.m b/src/g_calib/ext_calib.m new file mode 100755 index 0000000000000000000000000000000000000000..2c5093a77f232ac1325da03c1db21f351d4b9d07 --- /dev/null +++ b/src/g_calib/ext_calib.m @@ -0,0 +1,224 @@ + +%%%%%%%%%%%%%%%%%%%% SHOW EXTRINSIC RESULTS %%%%%%%%%%%%%%%%%%%%%%%% + +if ~exist('show_camera'), + show_camera = 1; +end; + + +if ~exist('n_ima')|~exist('fc'), + fprintf(1,'No calibration data available.\n'); + return; +end; + +check_active_images; + +if n_ima ~= 0, +if ~exist(['omc_' num2str(ind_active(1))]), + fprintf(1,'No calibration data available.\n'); + return; +end; +end; + +%if ~exist('no_grid'), + no_grid = 0; +%end; + +if n_ima ~= 0, +if ~exist(['n_sq_x_' num2str(ind_active(1))]), + no_grid = 1; +end; +else + no_grid = 1; +end; + +if ~exist('alpha_c'), + alpha_c = 0; +end; + + +if 0, + +err_std = std(ex'); + +fprintf(1,'\n\nCalibration results without principal point estimation:\n\n'); +fprintf(1,'Focal Length: fc = [ %3.5f %3.5f]\n',fc); +fprintf(1,'Principal point: cc = [ %3.5f %3.5f]\n',cc); +fprintf(1,'Distortion: kc = [ %3.5f %3.5f %3.5f %3.5f]\n',kc); +fprintf(1,'Pixel error: err = [ %3.5f %3.5f]\n\n',err_std); + +end; + + +% Color code for each image: + +colors = 'brgkcm'; + + +%%% Show the extrinsic parameters + +if n_ima ~= 0, +if ~exist('dX'), + eval(['dX = norm(Tc_' num2str(ind_active(1)) ')/10;']); + dY = dX; +end; +else + dX = 1; +end; + + +IP = 5*dX*[1 -alpha_c 0;0 1 0;0 0 1]*[1/fc(1) 0 0;0 1/fc(2) 0;0 0 1]*[1 0 -cc(1);0 1 -cc(2);0 0 1]*[0 nx-1 nx-1 0 0 ; 0 0 ny-1 ny-1 0;1 1 1 1 1]; +BASE = 5*dX*([0 1 0 0 0 0;0 0 0 1 0 0;0 0 0 0 0 1]); +IP = reshape([IP;BASE(:,1)*ones(1,5);IP],3,15); + +if ishandle(4), + figure(4); + [a,b] = view; +else + figure(4); + a = 50; + b = 20; +end; + + +if show_camera, + figure(4); + plot3(BASE(1,:),BASE(3,:),-BASE(2,:),'b-','linewidth',2); + hold on; + plot3(IP(1,:),IP(3,:),-IP(2,:),'r-','linewidth',2); + text(6*dX,0,0,'X_c'); + text(-dX,5*dX,0,'Z_c'); + text(0,0,-6*dX,'Y_c'); + text(-dX,-dX,dX,'O_c'); +else + figure(4); + clf; + hold on; +end; + + +for kk = 1:n_ima, + + if active_images(kk); + + if exist(['X_' num2str(kk)]) & exist(['omc_' num2str(kk)]), + + eval(['XX_kk = X_' num2str(kk) ';']); + + if ~isnan(XX_kk(1,1)) + + eval(['omc_kk = omc_' num2str(kk) ';']); + eval(['Tc_kk = Tc_' num2str(kk) ';']); + N_kk = size(XX_kk,2); + + if ~exist(['n_sq_x_' num2str(kk)]), + no_grid = 1; + else + eval(['n_sq_x = n_sq_x_' num2str(kk) ';']); + if isnan(n_sq_x(1)), + no_grid = 1; + end; + end; + + + if ~no_grid, + eval(['n_sq_x = n_sq_x_' num2str(kk) ';']); + eval(['n_sq_y = n_sq_y_' num2str(kk) ';']); + if (N_kk ~= ((n_sq_x+1)*(n_sq_y+1))), + no_grid = 1; + end; + end; + + if ~isnan(omc_kk(1,1)), + + R_kk = rodrigues(omc_kk); + + YY_kk = R_kk * XX_kk + Tc_kk * ones(1,length(XX_kk)); + + uu = [-dX;-dY;0]/2; + uu = R_kk * uu + Tc_kk; + + if ~no_grid, + YYx = zeros(n_sq_x+1,n_sq_y+1); + YYy = zeros(n_sq_x+1,n_sq_y+1); + YYz = zeros(n_sq_x+1,n_sq_y+1); + + YYx(:) = YY_kk(1,:); + YYy(:) = YY_kk(2,:); + YYz(:) = YY_kk(3,:); + + %keyboard; + + figure(4); + hhh= mesh(YYx,YYz,-YYy); + set(hhh,'edgecolor',colors(rem(kk-1,6)+1),'linewidth',1); %,'facecolor','none'); + %plot3(YY_kk(1,:),YY_kk(3,:),-YY_kk(2,:),['o' colors(rem(kk-1,6)+1)]); + text(uu(1),uu(3),-uu(2),num2str(kk),'fontsize',14,'color',colors(rem(kk-1,6)+1)); + else + + figure(4); + plot3(YY_kk(1,:),YY_kk(3,:),-YY_kk(2,:),['.' colors(rem(kk-1,6)+1)]); + text(uu(1),uu(3),-uu(2),num2str(kk),'fontsize',14,'color',colors(rem(kk-1,6)+1)); + + end; + + end; + + end; + + end; + + end; + +end; + +figure(4);rotate3d on; +axis('equal'); +title('Extrinsic parameters (camera-centered)'); +%view(60,30); +view(a,b); +grid on; +hold off; +axis vis3d; +axis tight; +set(4,'color',[1 1 1]); +if ~show_camera, + xlabel('X_c'); + ylabel('Z_c'); + zlabel('<-- Y_c'); +end; + +set(4,'Name','3D','NumberTitle','off'); + +%fprintf(1,'To generate the complete movie associated to the optimization loop, try: check_convergence;\n'); + + +if exist('h_switch2')==1, + if ishandle(h_switch2), + delete(h_switch2); + end; +end; + +if n_ima ~= 0, + if show_camera, + h_switch2 = uicontrol('Parent',4,'Units','normalized', 'Callback','show_camera=0;ext_calib;', 'Position',[1-.30 0.04 .30 .04],'String','Remove camera reference frame','fontsize',8,'fontname','clean','Tag','Pushbutton1'); + else + h_switch2 = uicontrol('Parent',4,'Units','normalized', 'Callback','show_camera=1;ext_calib;', 'Position',[1-.30 0.04 .30 .04],'String','Add camera reference frame','fontsize',8,'fontname','clean','Tag','Pushbutton1'); + end; +end; + + + + +if exist('h_switch')==1, + if ishandle(h_switch), + delete(h_switch); + end; +end; + +if n_ima ~= 0, +h_switch = uicontrol('Parent',4,'Units','normalized', 'Callback','ext_calib2', 'Position',[1-.30 0 .30 .04],'String','Switch to world-centered view','fontsize',8,'fontname','clean','Tag','Pushbutton1'); +end; + +figure(4); +rotate3d on; \ No newline at end of file diff --git a/src/g_calib/ext_calib2.m b/src/g_calib/ext_calib2.m new file mode 100755 index 0000000000000000000000000000000000000000..f056122dd004d82cbab8981248443e7646d23f17 --- /dev/null +++ b/src/g_calib/ext_calib2.m @@ -0,0 +1,234 @@ + +%%%%%%%%%%%%%%%%%%%% SHOW EXTRINSIC RESULTS %%%%%%%%%%%%%%%%%%%%%%%% + +if ~exist('show_camera'), + show_camera = 1; +end; + +if ~exist('n_ima')|~exist('fc'), + fprintf(1,'No calibration data available.\n'); + return; +end; + +check_active_images; + +if ~exist(['omc_' num2str(ind_active(1))]), + fprintf(1,'No calibration data available.\n'); + return; +end; + +%if ~exist('no_grid'), +no_grid = 0; +%end; + +if ~exist(['n_sq_x_' num2str(ind_active(1))]), + no_grid = 1; +end; + +if ~exist('alpha_c'), + alpha_c = 0; +end; + + +if 0, + + err_std = std(ex'); + + fprintf(1,'\n\nCalibration results without principal point estimation:\n\n'); + fprintf(1,'Focal Length: fc = [ %3.5f %3.5f]\n',fc); + fprintf(1,'Principal point: cc = [ %3.5f %3.5f]\n',cc); + fprintf(1,'Distortion: kc = [ %3.5f %3.5f %3.5f %3.5f]\n',kc); + fprintf(1,'Pixel error: err = [ %3.5f %3.5f]\n\n',err_std); + +end; + + +% Color code for each image: + +colors = 'brgkcm'; + + +%%% Show the extrinsic parameters + +if ~exist('dX'), + eval(['dX = norm(Tc_' num2str(ind_active(1)) ')/10;']); + dY = dX; +end; + +IP = 2*dX*[1 -alpha_c 0;0 1 0;0 0 1]*[1/fc(1) 0 0;0 1/fc(2) 0;0 0 1]*[1 0 -cc(1);0 1 -cc(2);0 0 1]*[0 nx-1 nx-1 0 0 ; 0 0 ny-1 ny-1 0;1 1 1 1 1]; +BASE = 2*(.9)*dX*([0 1 0 0 0 0;0 0 0 1 0 0;0 0 0 0 0 1]); +IP = reshape([IP;BASE(:,1)*ones(1,5);IP],3,15); +POS = [[6*dX;0;0] [0;6*dX;0] [-dX;0;5*dX] [-dX;-dX;-dX] [0;0;-dX]]; + + +if ishandle(4), + figure(4); + [a,b] = view; +else + figure(4); + a = 50; + b = 20; +end; + + +figure(4); +clf; +hold on; + + +for kk = 1:n_ima, + + if active_images(kk); + + if exist(['X_' num2str(kk)]) & exist(['omc_' num2str(kk)]), + + eval(['XX_kk = X_' num2str(kk) ';']); + + if ~isnan(XX_kk(1,1)) + + eval(['omc_kk = omc_' num2str(kk) ';']); + eval(['Tc_kk = Tc_' num2str(kk) ';']); + N_kk = size(XX_kk,2); + + if ~exist(['n_sq_x_' num2str(kk)]), + no_grid = 1; + else + eval(['n_sq_x = n_sq_x_' num2str(kk) ';']); + if isnan(n_sq_x(1)), + no_grid = 1; + end; + end; + + + if ~no_grid, + eval(['n_sq_x = n_sq_x_' num2str(kk) ';']); + eval(['n_sq_y = n_sq_y_' num2str(kk) ';']); + if (N_kk ~= ((n_sq_x+1)*(n_sq_y+1))), + no_grid = 1; + end; + end; + + if ~isnan(omc_kk(1,1)), + + R_kk = rodrigues(omc_kk); + + BASEk = R_kk'*(BASE - Tc_kk * ones(1,6)); + IPk = R_kk'*(IP - Tc_kk * ones(1,15)); + POSk = R_kk'*(POS - Tc_kk * ones(1,5)); + + YY_kk = XX_kk; + + if ~no_grid, + + YYx = zeros(n_sq_x+1,n_sq_y+1); + YYy = zeros(n_sq_x+1,n_sq_y+1); + YYz = zeros(n_sq_x+1,n_sq_y+1); + + YYx(:) = YY_kk(1,:); + YYy(:) = YY_kk(2,:); + YYz(:) = YY_kk(3,:); + + figure(4); + + if show_camera, + + p1 = struct('vertices',IPk','faces',[1 4 2;2 4 7;2 7 10;2 10 1]); + h1 = patch(p1); + set(h1,'facecolor',[52 217 160]/255,'EdgeColor', 'r'); + p2 = struct('vertices',IPk','faces',[1 10 7;7 4 1]); + h2 = patch(p2); + %set(h2,'facecolor',[236 171 76]/255,'EdgeColor', 'none'); + set(h2,'facecolor',[247 239 7]/255,'EdgeColor', 'none'); + + plot3(BASEk(1,:),BASEk(2,:),BASEk(3,:),'b-','linewidth',1'); + plot3(IPk(1,:),IPk(2,:),IPk(3,:),'r-','linewidth',1); + text(POSk(1,5),POSk(2,5),POSk(3,5),num2str(kk),'fontsize',10,'color','k','FontWeight','bold'); + + end; + + hhh= mesh(YYx,YYy,YYz); + set(hhh,'edgecolor',colors(rem(kk-1,6)+1),'linewidth',1); %,'facecolor','none'); + + else + + figure(4); + + if show_camera, + + p1 = struct('vertices',IPk','faces',[1 4 2;2 4 7;2 7 10;2 10 1]); + h1 = patch(p1); + set(h1,'facecolor',[52 217 160]/255,'EdgeColor', 'r'); + p2 = struct('vertices',IPk','faces',[1 10 7;7 4 1]); + h2 = patch(p2); + %set(h2,'facecolor',[236 171 76]/255,'EdgeColor', 'none'); + set(h2,'facecolor',[247 239 7]/255,'EdgeColor', 'none'); + + plot3(BASEk(1,:),BASEk(2,:),BASEk(3,:),'b-','linewidth',1'); + plot3(IPk(1,:),IPk(2,:),IPk(3,:),'r-','linewidth',1); + hww = text(POSk(1,5),POSk(2,5),POSk(3,5),num2str(kk),'fontsize',10,'color','k','FontWeight','bold'); + + end; + + plot3(YY_kk(1,:),YY_kk(2,:),YY_kk(3,:),['.' colors(rem(kk-1,6)+1)]); + + end; + + end; + + end; + + end; + + end; + +end; + +figure(4);rotate3d on; +axis('equal'); +title('Extrinsic parameters (world-centered)'); +%view(60,30); +xlabel('X_{world}') +ylabel('Y_{world}') +zlabel('Z_{world}') +view(a,b); +axis vis3d; +axis tight; +grid on; +plot3(3*dX*[1 0 0 0 0],3*dX*[0 0 1 0 0],3*dX*[0 0 0 0 1],'r-','linewidth',3); +hold off; +set(4,'color',[1 1 1]); + +set(4,'Name','3D','NumberTitle','off'); + +%hh = axis; +%hh(5) = 0; +%axis(hh); + +%fprintf(1,'To generate the complete movie associated to the optimization loop, try: check_convergence;\n'); + +if exist('h_switch2')==1, + if ishandle(h_switch2), + delete(h_switch2); + end; +end; + +if n_ima ~= 0, + if show_camera, + h_switch2 = uicontrol('Parent',4,'Units','normalized', 'Callback','show_camera=0;ext_calib2;', 'Position',[1-.30 0.04 .30 .04],'String','Remove camera reference frames','fontsize',8,'fontname','clean','Tag','Pushbutton1'); + else + h_switch2 = uicontrol('Parent',4,'Units','normalized', 'Callback','show_camera=1;ext_calib2;', 'Position',[1-.30 0.04 .30 .04],'String','Add camera reference frames','fontsize',8,'fontname','clean','Tag','Pushbutton1'); + end; +end; + + + +if exist('h_switch')==1, + if ishandle(h_switch), + delete(h_switch); + end; +end; + +h_switch = uicontrol('Parent',4,'Units','normalized', 'Callback','ext_calib', 'Position',[1-.30 0 .30 .04],'String','Switch to camera-centered view','fontsize',8,'fontname','clean','Tag','Pushbutton1'); + +figure(4); +rotate3d on; \ No newline at end of file diff --git a/src/g_calib/ext_calib_stereo.m b/src/g_calib/ext_calib_stereo.m new file mode 100755 index 0000000000000000000000000000000000000000..c12cfd9144f8c503adfa6fd36abd519c5587d3be --- /dev/null +++ b/src/g_calib/ext_calib_stereo.m @@ -0,0 +1,169 @@ + +%%%%%%%%%%%%%%%%%%%% SHOW EXTRINSIC RESULTS %%%%%%%%%%%%%%%%%%%%%%%% + +if ~exist('n_ima')|~exist('fc_right')|~exist('fc_left'), + fprintf(1,'No stereo calibration data available.\n'); + return; +end; + +ind_active = find(active_images); + +no_grid = 0; + +if ~exist(['n_sq_x_' num2str(ind_active(1))]), + no_grid = 1; +end; + +% Color code for each image: + +colors = 'brgkcm'; + + +%%% Show the extrinsic parameters + +if ~exist('dX'), + eval(['dX = norm(Tc_left_' num2str(ind_active(1)) ')/10;']); + dY = dX; +end; + +%normT = min(norm(T)/2,2*dX); +normT = 2*dX; + + +IP_left = normT *[1 -alpha_c_left 0;0 1 0;0 0 1]*[1/fc_left(1) 0 0;0 1/fc_left(2) 0;0 0 1]*[1 0 -cc_left(1);0 1 -cc_left(2);0 0 1]*[0 nx-1 nx-1 0 0 ; 0 0 ny-1 ny-1 0;1 1 1 1 1]; +BASE_left = normT *([0 1 0 0 0 0;0 0 0 1 0 0;0 0 0 0 0 1]); +IP_left = reshape([IP_left ;BASE_left(:,1)*ones(1,5);IP_left ],3,15); + +IP_right = normT *[1 -alpha_c_right 0;0 1 0;0 0 1]*[1/fc_right(1) 0 0;0 1/fc_right(2) 0;0 0 1]*[1 0 -cc_right(1);0 1 -cc_right(2);0 0 1]*[0 nx-1 nx-1 0 0 ; 0 0 ny-1 ny-1 0;1 1 1 1 1]; +IP_right = reshape([IP_right;BASE_left(:,1)*ones(1,5);IP_right],3,15); + +% Relative position of right camera wrt left camera: (om,T) +R = rodrigues(om); + + +% Change of reference: +BASE_right = R'*(BASE_left - repmat(T,[1 6])); +IP_right = R'*(IP_right - repmat(T,[1 15])); + + +if ishandle(4), + figure(4); + [a,b] = view; +else + figure(4); + a = 50; + b = 20; +end; + + +figure(4); +plot3(BASE_left(1,:),BASE_left(3,:),-BASE_left(2,:),'b-','linewidth',2'); +hold on; +plot3(IP_left(1,:),IP_left(3,:),-IP_left(2,:),'r-','linewidth',2); +text(BASE_left(1,2),BASE_left(3,2),-BASE_left(2,2),'X','HorizontalAlignment','center','FontWeight','bold'); +text(BASE_left(1,6),BASE_left(3,6),-BASE_left(2,6),'Z','HorizontalAlignment','center','FontWeight','bold'); +text(BASE_left(1,4),BASE_left(3,4),-BASE_left(2,4),'Y','HorizontalAlignment','center','FontWeight','bold'); +text(BASE_left(1,1),BASE_left(3,1),-BASE_left(2,1),'Left Camera','HorizontalAlignment','center','FontWeight','bold'); +plot3(BASE_right(1,:),BASE_right(3,:),-BASE_right(2,:),'b-','linewidth',2'); +plot3(IP_right(1,:),IP_right(3,:),-IP_right(2,:),'r-','linewidth',2); +text(BASE_right(1,2),BASE_right(3,2),-BASE_right(2,2),'X','HorizontalAlignment','center','FontWeight','bold'); +text(BASE_right(1,6),BASE_right(3,6),-BASE_right(2,6),'Z','HorizontalAlignment','center','FontWeight','bold'); +text(BASE_right(1,4),BASE_right(3,4),-BASE_right(2,4),'Y','HorizontalAlignment','center','FontWeight','bold'); +text(BASE_right(1,1),BASE_right(3,1),-BASE_right(2,1),'Right Camera','HorizontalAlignment','center','FontWeight','bold'); + +for kk = 1:n_ima, + + if active_images(kk); + + if exist(['X_left_' num2str(kk)]) & exist(['omc_left_' num2str(kk)]), + + eval(['XX_kk = X_left_' num2str(kk) ';']); + + if ~isnan(XX_kk(1,1)) + + eval(['omc_kk = omc_left_' num2str(kk) ';']); + eval(['Tc_kk = Tc_left_' num2str(kk) ';']); + N_kk = size(XX_kk,2); + + if ~exist(['n_sq_x_' num2str(kk)]), + no_grid = 1; + else + eval(['n_sq_x = n_sq_x_' num2str(kk) ';']); + if isnan(n_sq_x(1)), + no_grid = 1; + end; + end; + + if ~no_grid, + eval(['n_sq_x = n_sq_x_' num2str(kk) ';']); + eval(['n_sq_y = n_sq_y_' num2str(kk) ';']); + if (N_kk ~= ((n_sq_x+1)*(n_sq_y+1))), + no_grid = 1; + end; + end; + + if ~isnan(omc_kk(1,1)), + + R_kk = rodrigues(omc_kk); + + YY_kk = R_kk * XX_kk + Tc_kk * ones(1,length(XX_kk)); + + uu = [-dX;-dY;0]/2; + uu = R_kk * uu + Tc_kk; + + if ~no_grid, + YYx = zeros(n_sq_x+1,n_sq_y+1); + YYy = zeros(n_sq_x+1,n_sq_y+1); + YYz = zeros(n_sq_x+1,n_sq_y+1); + + YYx(:) = YY_kk(1,:); + YYy(:) = YY_kk(2,:); + YYz(:) = YY_kk(3,:); + + + figure(4); + hhh= mesh(YYx,YYz,-YYy); + set(hhh,'edgecolor',colors(rem(kk-1,6)+1),'linewidth',1); %,'facecolor','none'); + text(uu(1),uu(3),-uu(2),num2str(kk),'fontsize',14,'color',colors(rem(kk-1,6)+1),'HorizontalAlignment','center'); + else + + figure(4); + plot3(YY_kk(1,:),YY_kk(3,:),-YY_kk(2,:),['.' colors(rem(kk-1,6)+1)]); + text(uu(1),uu(3),-uu(2),num2str(kk),'fontsize',14,'color',colors(rem(kk-1,6)+1),'HorizontalAlignment','center'); + + end; + + end; + + end; + + end; + + end; + +end; + +figure(4);rotate3d on; +axis('equal'); +title('Extrinsic parameters'); +view(a,b); +grid on; +hold off; +axis vis3d; +axis tight; +set(4,'color',[1 1 1]); + +set(4,'Name','3D','NumberTitle','off'); + + +if exist('h_switch')==1, + if ishandle(h_switch), + delete(h_switch); + end; +end; + +if exist('h_switch2')==1, + if ishandle(h_switch2), + delete(h_switch2); + end; +end; \ No newline at end of file diff --git a/src/g_calib/extract_distortion_data.m b/src/g_calib/extract_distortion_data.m new file mode 100755 index 0000000000000000000000000000000000000000..b6f8c071da3701a8936e5fbe9fd8a4ad06dea504 --- /dev/null +++ b/src/g_calib/extract_distortion_data.m @@ -0,0 +1,46 @@ +%%% Small script file that etst + +%load Calib_Results; + +% Collect all the points distorted (xd) and undistorted (xn) from the +% images: + +xn = []; +xd = []; +for kk = ind_active, + eval(['x_kk = x_' num2str(kk) ';']); + xd_kk = normalize_pixel(x_kk,fc,cc,zeros(5,1),alpha_c); + eval(['X_kk = X_' num2str(kk) ';']); + eval(['omckk = omc_' num2str(kk) ';']); + eval(['Tckk = Tc_' num2str(kk) ';']); + xn_kk = project_points2(X_kk,omckk,Tckk); + xd = [xd xd_kk]; + xn = [xn xn_kk]; +end; + + +% Data points: +r = sqrt(sum(xn.^2)); % The undistorted radii +rp = sqrt(sum(xd.^2)); % The distorted radii + +%--- Try different analytical models to fit r_prime = D(r) + +ri = 0.005:.005:max(r); + +% Calibration toolbox model: +rt = ri .* (1 + kc(1)*ri.^2 + kc(2)*ri.^4 + kc(5)*ri.^6); + + + +return; + + +figure(10); +clf; +h1 = plot(r,rp,'r.','markersize',.1); hold on; +h2 = plot(ri,rt,'r-','linewidth',.1); +title('Radial distortion function (with unit focal) - r prime = D(r)'); +xlabel('r'); +ylabel('r prime'); +zoom on; + diff --git a/src/g_calib/extract_grid.m b/src/g_calib/extract_grid.m new file mode 100755 index 0000000000000000000000000000000000000000..1b1936e24147858308f11608ec9a017a9fa46975 --- /dev/null +++ b/src/g_calib/extract_grid.m @@ -0,0 +1,329 @@ +function [x,X,n_sq_x,n_sq_y,ind_orig,ind_x,ind_y] = extract_grid(I,wintx,winty,fc,cc,kc,dX,dY,xr,yr,click_mode); + +if nargin < 11, + click_mode = 0; +end; + + +if nargin < 10, + xr = []; + yr = []; +end; + + +map = gray(256); + +minI = min(I(:)); +maxI = max(I(:)); + +Id = 255*(I - minI)/(maxI - minI); + +figure(2); +image(Id); +colormap(map); + +if ~isempty(xr); + figure(2); + hold on; + plot(xr,yr,'go'); + hold off; +end; + + +if nargin < 2, + + disp('Window size for corner finder (wintx and winty):'); + wintx = input('wintx ([] = 5) = '); + if isempty(wintx), wintx = 5; end; + wintx = round(wintx); + winty = input('winty ([] = 5) = '); + if isempty(winty), winty = 5; end; + winty = round(winty); + + fprintf(1,'Window size = %dx%d\n',2*wintx+1,2*winty+1); + +end; + + +need_to_click = 1; + +color_line = 'g'; + +while need_to_click, + + + title('Click on the four extreme corners of the rectangular pattern (first corner = origin)...'); + + disp('Click on the four extreme corners of the rectangular complete pattern (the first clicked corner is the origin)...'); + + x= [];y = []; + figure(2); hold on; + for count = 1:4, + [xi,yi] = ginput4(1); + [xxi] = cornerfinder([xi;yi],I,winty,wintx); + xi = xxi(1); + yi = xxi(2); + figure(2); + plot(xi,yi,'+','color',[ 1.000 0.314 0.510 ],'linewidth',2); + plot(xi + [wintx+.5 -(wintx+.5) -(wintx+.5) wintx+.5 wintx+.5],yi + [winty+.5 winty+.5 -(winty+.5) -(winty+.5) winty+.5],'-','color',[ 1.000 0.314 0.510 ],'linewidth',2); + x = [x;xi]; + y = [y;yi]; + plot(x,y,'-','color',[ 1.000 0.314 0.510 ],'linewidth',2); + drawnow; + end; + plot([x;x(1)],[y;y(1)],'-','color',[ 1.000 0.314 0.510 ],'linewidth',2); + drawnow; + hold off; + + [Xc,good,bad,type] = cornerfinder([x';y'],I,winty,wintx); % the four corners + + x = Xc(1,:)'; + y = Xc(2,:)'; + + + % Sort the corners: + x_mean = mean(x); + y_mean = mean(y); + x_v = x - x_mean; + y_v = y - y_mean; + + theta = atan2(-y_v,x_v); + [junk,ind] = sort(theta); + + [junk,ind] = sort(mod(theta-theta(1),2*pi)); + + %ind = ind([2 3 4 1]); + + ind = ind([4 3 2 1]); %-> New: the Z axis is pointing uppward + + + x = x(ind); + y = y(ind); + x1= x(1); x2 = x(2); x3 = x(3); x4 = x(4); + y1= y(1); y2 = y(2); y3 = y(3); y4 = y(4); + + + % Find center: + p_center = cross(cross([x1;y1;1],[x3;y3;1]),cross([x2;y2;1],[x4;y4;1])); + x5 = p_center(1)/p_center(3); + y5 = p_center(2)/p_center(3); + + % center on the X axis: + x6 = (x3 + x4)/2; + y6 = (y3 + y4)/2; + + % center on the Y axis: + x7 = (x1 + x4)/2; + y7 = (y1 + y4)/2; + + % Direction of displacement for the X axis: + vX = [x6-x5;y6-y5]; + vX = vX / norm(vX); + + % Direction of displacement for the X axis: + vY = [x7-x5;y7-y5]; + vY = vY / norm(vY); + + % Direction of diagonal: + vO = [x4 - x5; y4 - y5]; + vO = vO / norm(vO); + + delta = 30; + + + figure(2); image(Id); + colormap(map); + hold on; + plot([x;x(1)],[y;y(1)],'g-'); + plot(x,y,'og'); + hx=text(x6 + delta * vX(1) ,y6 + delta*vX(2),'X'); + set(hx,'color','g','Fontsize',14); + hy=text(x7 + delta*vY(1), y7 + delta*vY(2),'Y'); + set(hy,'color','g','Fontsize',14); + hO=text(x4 + delta * vO(1) ,y4 + delta*vO(2),'O','color','g','Fontsize',14); + hold off; + + + % Try to automatically count the number of squares in the grid + + n_sq_x1 = count_squares(I,x1,y1,x2,y2,wintx); + n_sq_x2 = count_squares(I,x3,y3,x4,y4,wintx); + n_sq_y1 = count_squares(I,x2,y2,x3,y3,wintx); + n_sq_y2 = count_squares(I,x4,y4,x1,y1,wintx); + + + + % If could not count the number of squares, enter manually + + if (n_sq_x1~=n_sq_x2)|(n_sq_y1~=n_sq_y2), + + if ~click_mode, + + % This way, the user manually enters the number of squares and no more clicks. + % Otherwise, he user is asked to click again. + + disp('Could not count the number of squares in the grid. Enter manually.'); + n_sq_x = input('Number of squares along the X direction ([]=10) = '); %6 + if isempty(n_sq_x), n_sq_x = 10; end; + n_sq_y = input('Number of squares along the Y direction ([]=10) = '); %6 + if isempty(n_sq_y), n_sq_y = 10; end; + need_to_click = 0; + + end; + + + else + + n_sq_x = n_sq_x1; + n_sq_y = n_sq_y1; + + need_to_click = 0; + + end; + + color_line = 'r'; + +end; + + +if ~exist('dX')|~exist('dY'), + + % Enter the size of each square + + dX = input(['Size dX of each square along the X direction ([]=30mm) = ']); + dY = input(['Size dY of each square along the Y direction ([]=30mm) = ']); + if isempty(dX), dX = 30; end; + if isempty(dY), dY = 30; end; + +end; + + +% Compute the inside points through computation of the planar homography (collineation) + +a00 = [x(1);y(1);1]; +a10 = [x(2);y(2);1]; +a11 = [x(3);y(3);1]; +a01 = [x(4);y(4);1]; + + +% Compute the planar collineation: (return the normalization matrice as well) + +[Homo,Hnorm,inv_Hnorm] = compute_homography ([a00 a10 a11 a01],[0 1 1 0;0 0 1 1;1 1 1 1]); + + +% Build the grid using the planar collineation: + +x_l = ((0:n_sq_x)'*ones(1,n_sq_y+1))/n_sq_x; +y_l = (ones(n_sq_x+1,1)*(0:n_sq_y))/n_sq_y; +pts = [x_l(:) y_l(:) ones((n_sq_x+1)*(n_sq_y+1),1)]'; + +XX = Homo*pts; +XX = XX(1:2,:) ./ (ones(2,1)*XX(3,:)); + + +% Complete size of the rectangle + +W = n_sq_x*dX; +L = n_sq_y*dY; + + + +if nargin < 6, + + %%%%%%%%%%%%%%%%%%%%%%%% ADDITIONAL STUFF IN THE CASE OF HIGHLY DISTORTED IMAGES %%%%%%%%%%%%% + figure(2); + hold on; + plot(XX(1,:),XX(2,:),'r+'); + title('The red crosses should be close to the image corners'); + hold off; + + disp('If the guessed grid corners (red crosses on the image) are not close to the actual corners,'); + disp('it is necessary to enter an initial guess for the radial distortion factor kc (useful for subpixel detection)'); + quest_distort = input('Need of an initial guess for distortion? ([]=no, other=yes) '); + + quest_distort = ~isempty(quest_distort); + + if quest_distort, + % Estimation of focal length: + c_g = [size(I,2);size(I,1)]/2 + .5; + f_g = Distor2Calib(0,[[x(1) x(2) x(4) x(3)] - c_g(1);[y(1) y(2) y(4) y(3)] - c_g(2)],1,1,4,W,L,[-W/2 W/2 W/2 -W/2;L/2 L/2 -L/2 -L/2; 0 0 0 0],100,1,1); + f_g = mean(f_g); + script_fit_distortion; + end; + %%%%%%%%%%%%%%%%%%%%% END ADDITIONAL STUFF IN THE CASE OF HIGHLY DISTORTED IMAGES %%%%%%%%%%%%% + +else + + xy_corners_undist = comp_distortion_oulu([(x' - cc(1))/fc(1);(y'-cc(2))/fc(2)],kc); + + xu = xy_corners_undist(1,:)'; + yu = xy_corners_undist(2,:)'; + + [XXu] = projectedGrid ( [xu(1);yu(1)], [xu(2);yu(2)],[xu(3);yu(3)], [xu(4);yu(4)],n_sq_x+1,n_sq_y+1); % The full grid + + r2 = sum(XXu.^2); + XX = (ones(2,1)*(1 + kc(1) * r2 + kc(2) * (r2.^2))) .* XXu; + XX(1,:) = fc(1)*XX(1,:)+cc(1); + XX(2,:) = fc(2)*XX(2,:)+cc(2); + +end; + + +Np = (n_sq_x+1)*(n_sq_y+1); + +disp('Corner extraction...'); + +grid_pts = cornerfinder(XX,I,winty,wintx); %%% Finds the exact corners at every points! + +grid_pts = grid_pts - 1; % subtract 1 to bring the origin to (0,0) instead of (1,1) in matlab (not necessary in C) + +ind_corners = [1 n_sq_x+1 (n_sq_x+1)*n_sq_y+1 (n_sq_x+1)*(n_sq_y+1)]; % index of the 4 corners +ind_orig = (n_sq_x+1)*n_sq_y + 1; +xorig = grid_pts(1,ind_orig); +yorig = grid_pts(2,ind_orig); +dxpos = mean([grid_pts(:,ind_orig) grid_pts(:,ind_orig+1)]'); +dypos = mean([grid_pts(:,ind_orig) grid_pts(:,ind_orig-n_sq_x-1)]'); + + +ind_x = (n_sq_x+1)*(n_sq_y + 1); +ind_y = 1; + +x_box_kk = [grid_pts(1,:)-(wintx+.5);grid_pts(1,:)+(wintx+.5);grid_pts(1,:)+(wintx+.5);grid_pts(1,:)-(wintx+.5);grid_pts(1,:)-(wintx+.5)]; +y_box_kk = [grid_pts(2,:)-(winty+.5);grid_pts(2,:)-(winty+.5);grid_pts(2,:)+(winty+.5);grid_pts(2,:)+(winty+.5);grid_pts(2,:)-(winty+.5)]; + + + + +figure(3); +image(Id); colormap(map); hold on; +plot(grid_pts(1,:)+1,grid_pts(2,:)+1,'r+'); +plot(x_box_kk+1,y_box_kk+1,'-b'); +plot(grid_pts(1,ind_corners)+1,grid_pts(2,ind_corners)+1,'mo'); +plot(xorig+1,yorig+1,'*m'); +h = text(xorig+delta*vO(1),yorig+delta*vO(2),'O'); +set(h,'Color','m','FontSize',14); +h2 = text(dxpos(1)+delta*vX(1),dxpos(2)+delta*vX(2),'dX'); +set(h2,'Color','g','FontSize',14); +h3 = text(dypos(1)+delta*vY(1),dypos(2)+delta*vY(2),'dY'); +set(h3,'Color','g','FontSize',14); +xlabel('Xc (in camera frame)'); +ylabel('Yc (in camera frame)'); +title('Extracted corners'); +zoom on; +drawnow; +hold off; + + + +Xi = reshape(([0:n_sq_x]*dX)'*ones(1,n_sq_y+1),Np,1)'; +Yi = reshape(ones(n_sq_x+1,1)*[n_sq_y:-1:0]*dY,Np,1)'; +Zi = zeros(1,Np); + +Xgrid = [Xi;Yi;Zi]; + + +% All the point coordinates (on the image, and in 3D) - for global optimization: + +x = grid_pts; +X = Xgrid; + diff --git a/src/g_calib/extract_grid_manual.m b/src/g_calib/extract_grid_manual.m new file mode 100755 index 0000000000000000000000000000000000000000..1b1936e24147858308f11608ec9a017a9fa46975 --- /dev/null +++ b/src/g_calib/extract_grid_manual.m @@ -0,0 +1,329 @@ +function [x,X,n_sq_x,n_sq_y,ind_orig,ind_x,ind_y] = extract_grid(I,wintx,winty,fc,cc,kc,dX,dY,xr,yr,click_mode); + +if nargin < 11, + click_mode = 0; +end; + + +if nargin < 10, + xr = []; + yr = []; +end; + + +map = gray(256); + +minI = min(I(:)); +maxI = max(I(:)); + +Id = 255*(I - minI)/(maxI - minI); + +figure(2); +image(Id); +colormap(map); + +if ~isempty(xr); + figure(2); + hold on; + plot(xr,yr,'go'); + hold off; +end; + + +if nargin < 2, + + disp('Window size for corner finder (wintx and winty):'); + wintx = input('wintx ([] = 5) = '); + if isempty(wintx), wintx = 5; end; + wintx = round(wintx); + winty = input('winty ([] = 5) = '); + if isempty(winty), winty = 5; end; + winty = round(winty); + + fprintf(1,'Window size = %dx%d\n',2*wintx+1,2*winty+1); + +end; + + +need_to_click = 1; + +color_line = 'g'; + +while need_to_click, + + + title('Click on the four extreme corners of the rectangular pattern (first corner = origin)...'); + + disp('Click on the four extreme corners of the rectangular complete pattern (the first clicked corner is the origin)...'); + + x= [];y = []; + figure(2); hold on; + for count = 1:4, + [xi,yi] = ginput4(1); + [xxi] = cornerfinder([xi;yi],I,winty,wintx); + xi = xxi(1); + yi = xxi(2); + figure(2); + plot(xi,yi,'+','color',[ 1.000 0.314 0.510 ],'linewidth',2); + plot(xi + [wintx+.5 -(wintx+.5) -(wintx+.5) wintx+.5 wintx+.5],yi + [winty+.5 winty+.5 -(winty+.5) -(winty+.5) winty+.5],'-','color',[ 1.000 0.314 0.510 ],'linewidth',2); + x = [x;xi]; + y = [y;yi]; + plot(x,y,'-','color',[ 1.000 0.314 0.510 ],'linewidth',2); + drawnow; + end; + plot([x;x(1)],[y;y(1)],'-','color',[ 1.000 0.314 0.510 ],'linewidth',2); + drawnow; + hold off; + + [Xc,good,bad,type] = cornerfinder([x';y'],I,winty,wintx); % the four corners + + x = Xc(1,:)'; + y = Xc(2,:)'; + + + % Sort the corners: + x_mean = mean(x); + y_mean = mean(y); + x_v = x - x_mean; + y_v = y - y_mean; + + theta = atan2(-y_v,x_v); + [junk,ind] = sort(theta); + + [junk,ind] = sort(mod(theta-theta(1),2*pi)); + + %ind = ind([2 3 4 1]); + + ind = ind([4 3 2 1]); %-> New: the Z axis is pointing uppward + + + x = x(ind); + y = y(ind); + x1= x(1); x2 = x(2); x3 = x(3); x4 = x(4); + y1= y(1); y2 = y(2); y3 = y(3); y4 = y(4); + + + % Find center: + p_center = cross(cross([x1;y1;1],[x3;y3;1]),cross([x2;y2;1],[x4;y4;1])); + x5 = p_center(1)/p_center(3); + y5 = p_center(2)/p_center(3); + + % center on the X axis: + x6 = (x3 + x4)/2; + y6 = (y3 + y4)/2; + + % center on the Y axis: + x7 = (x1 + x4)/2; + y7 = (y1 + y4)/2; + + % Direction of displacement for the X axis: + vX = [x6-x5;y6-y5]; + vX = vX / norm(vX); + + % Direction of displacement for the X axis: + vY = [x7-x5;y7-y5]; + vY = vY / norm(vY); + + % Direction of diagonal: + vO = [x4 - x5; y4 - y5]; + vO = vO / norm(vO); + + delta = 30; + + + figure(2); image(Id); + colormap(map); + hold on; + plot([x;x(1)],[y;y(1)],'g-'); + plot(x,y,'og'); + hx=text(x6 + delta * vX(1) ,y6 + delta*vX(2),'X'); + set(hx,'color','g','Fontsize',14); + hy=text(x7 + delta*vY(1), y7 + delta*vY(2),'Y'); + set(hy,'color','g','Fontsize',14); + hO=text(x4 + delta * vO(1) ,y4 + delta*vO(2),'O','color','g','Fontsize',14); + hold off; + + + % Try to automatically count the number of squares in the grid + + n_sq_x1 = count_squares(I,x1,y1,x2,y2,wintx); + n_sq_x2 = count_squares(I,x3,y3,x4,y4,wintx); + n_sq_y1 = count_squares(I,x2,y2,x3,y3,wintx); + n_sq_y2 = count_squares(I,x4,y4,x1,y1,wintx); + + + + % If could not count the number of squares, enter manually + + if (n_sq_x1~=n_sq_x2)|(n_sq_y1~=n_sq_y2), + + if ~click_mode, + + % This way, the user manually enters the number of squares and no more clicks. + % Otherwise, he user is asked to click again. + + disp('Could not count the number of squares in the grid. Enter manually.'); + n_sq_x = input('Number of squares along the X direction ([]=10) = '); %6 + if isempty(n_sq_x), n_sq_x = 10; end; + n_sq_y = input('Number of squares along the Y direction ([]=10) = '); %6 + if isempty(n_sq_y), n_sq_y = 10; end; + need_to_click = 0; + + end; + + + else + + n_sq_x = n_sq_x1; + n_sq_y = n_sq_y1; + + need_to_click = 0; + + end; + + color_line = 'r'; + +end; + + +if ~exist('dX')|~exist('dY'), + + % Enter the size of each square + + dX = input(['Size dX of each square along the X direction ([]=30mm) = ']); + dY = input(['Size dY of each square along the Y direction ([]=30mm) = ']); + if isempty(dX), dX = 30; end; + if isempty(dY), dY = 30; end; + +end; + + +% Compute the inside points through computation of the planar homography (collineation) + +a00 = [x(1);y(1);1]; +a10 = [x(2);y(2);1]; +a11 = [x(3);y(3);1]; +a01 = [x(4);y(4);1]; + + +% Compute the planar collineation: (return the normalization matrice as well) + +[Homo,Hnorm,inv_Hnorm] = compute_homography ([a00 a10 a11 a01],[0 1 1 0;0 0 1 1;1 1 1 1]); + + +% Build the grid using the planar collineation: + +x_l = ((0:n_sq_x)'*ones(1,n_sq_y+1))/n_sq_x; +y_l = (ones(n_sq_x+1,1)*(0:n_sq_y))/n_sq_y; +pts = [x_l(:) y_l(:) ones((n_sq_x+1)*(n_sq_y+1),1)]'; + +XX = Homo*pts; +XX = XX(1:2,:) ./ (ones(2,1)*XX(3,:)); + + +% Complete size of the rectangle + +W = n_sq_x*dX; +L = n_sq_y*dY; + + + +if nargin < 6, + + %%%%%%%%%%%%%%%%%%%%%%%% ADDITIONAL STUFF IN THE CASE OF HIGHLY DISTORTED IMAGES %%%%%%%%%%%%% + figure(2); + hold on; + plot(XX(1,:),XX(2,:),'r+'); + title('The red crosses should be close to the image corners'); + hold off; + + disp('If the guessed grid corners (red crosses on the image) are not close to the actual corners,'); + disp('it is necessary to enter an initial guess for the radial distortion factor kc (useful for subpixel detection)'); + quest_distort = input('Need of an initial guess for distortion? ([]=no, other=yes) '); + + quest_distort = ~isempty(quest_distort); + + if quest_distort, + % Estimation of focal length: + c_g = [size(I,2);size(I,1)]/2 + .5; + f_g = Distor2Calib(0,[[x(1) x(2) x(4) x(3)] - c_g(1);[y(1) y(2) y(4) y(3)] - c_g(2)],1,1,4,W,L,[-W/2 W/2 W/2 -W/2;L/2 L/2 -L/2 -L/2; 0 0 0 0],100,1,1); + f_g = mean(f_g); + script_fit_distortion; + end; + %%%%%%%%%%%%%%%%%%%%% END ADDITIONAL STUFF IN THE CASE OF HIGHLY DISTORTED IMAGES %%%%%%%%%%%%% + +else + + xy_corners_undist = comp_distortion_oulu([(x' - cc(1))/fc(1);(y'-cc(2))/fc(2)],kc); + + xu = xy_corners_undist(1,:)'; + yu = xy_corners_undist(2,:)'; + + [XXu] = projectedGrid ( [xu(1);yu(1)], [xu(2);yu(2)],[xu(3);yu(3)], [xu(4);yu(4)],n_sq_x+1,n_sq_y+1); % The full grid + + r2 = sum(XXu.^2); + XX = (ones(2,1)*(1 + kc(1) * r2 + kc(2) * (r2.^2))) .* XXu; + XX(1,:) = fc(1)*XX(1,:)+cc(1); + XX(2,:) = fc(2)*XX(2,:)+cc(2); + +end; + + +Np = (n_sq_x+1)*(n_sq_y+1); + +disp('Corner extraction...'); + +grid_pts = cornerfinder(XX,I,winty,wintx); %%% Finds the exact corners at every points! + +grid_pts = grid_pts - 1; % subtract 1 to bring the origin to (0,0) instead of (1,1) in matlab (not necessary in C) + +ind_corners = [1 n_sq_x+1 (n_sq_x+1)*n_sq_y+1 (n_sq_x+1)*(n_sq_y+1)]; % index of the 4 corners +ind_orig = (n_sq_x+1)*n_sq_y + 1; +xorig = grid_pts(1,ind_orig); +yorig = grid_pts(2,ind_orig); +dxpos = mean([grid_pts(:,ind_orig) grid_pts(:,ind_orig+1)]'); +dypos = mean([grid_pts(:,ind_orig) grid_pts(:,ind_orig-n_sq_x-1)]'); + + +ind_x = (n_sq_x+1)*(n_sq_y + 1); +ind_y = 1; + +x_box_kk = [grid_pts(1,:)-(wintx+.5);grid_pts(1,:)+(wintx+.5);grid_pts(1,:)+(wintx+.5);grid_pts(1,:)-(wintx+.5);grid_pts(1,:)-(wintx+.5)]; +y_box_kk = [grid_pts(2,:)-(winty+.5);grid_pts(2,:)-(winty+.5);grid_pts(2,:)+(winty+.5);grid_pts(2,:)+(winty+.5);grid_pts(2,:)-(winty+.5)]; + + + + +figure(3); +image(Id); colormap(map); hold on; +plot(grid_pts(1,:)+1,grid_pts(2,:)+1,'r+'); +plot(x_box_kk+1,y_box_kk+1,'-b'); +plot(grid_pts(1,ind_corners)+1,grid_pts(2,ind_corners)+1,'mo'); +plot(xorig+1,yorig+1,'*m'); +h = text(xorig+delta*vO(1),yorig+delta*vO(2),'O'); +set(h,'Color','m','FontSize',14); +h2 = text(dxpos(1)+delta*vX(1),dxpos(2)+delta*vX(2),'dX'); +set(h2,'Color','g','FontSize',14); +h3 = text(dypos(1)+delta*vY(1),dypos(2)+delta*vY(2),'dY'); +set(h3,'Color','g','FontSize',14); +xlabel('Xc (in camera frame)'); +ylabel('Yc (in camera frame)'); +title('Extracted corners'); +zoom on; +drawnow; +hold off; + + + +Xi = reshape(([0:n_sq_x]*dX)'*ones(1,n_sq_y+1),Np,1)'; +Yi = reshape(ones(n_sq_x+1,1)*[n_sq_y:-1:0]*dY,Np,1)'; +Zi = zeros(1,Np); + +Xgrid = [Xi;Yi;Zi]; + + +% All the point coordinates (on the image, and in 3D) - for global optimization: + +x = grid_pts; +X = Xgrid; + diff --git a/src/g_calib/extract_parameters.m b/src/g_calib/extract_parameters.m new file mode 100755 index 0000000000000000000000000000000000000000..71dcbc93272c11f0e644a887c2fd2378a0520428 --- /dev/null +++ b/src/g_calib/extract_parameters.m @@ -0,0 +1,61 @@ + +%%% Extraction of the final intrinsic and extrinsic paramaters: + +check_active_images; + +if ~exist('solution_error') + solution_error = zeros(6*n_ima + 15,1); +end; + +fc = solution(1:2);%*** +cc = solution(3:4);%*** +alpha_c = solution(5);%*** +kc = solution(6:10);%*** + +fc_error = solution_error(1:2); +cc_error = solution_error(3:4); +alpha_c_error = solution_error(5); +kc_error = solution_error(6:10); + +% Calibration matrix: + +KK = [fc(1) fc(1)*alpha_c cc(1);0 fc(2) cc(2); 0 0 1]; +inv_KK = inv(KK); + +% Extract the extrinsic paramters, and recomputer the collineations + +for kk = 1:n_ima, + + if active_images(kk), + + omckk = solution(15+6*(kk-1) + 1:15+6*(kk-1) + 3);%*** + Tckk = solution(15+6*(kk-1) + 4:15+6*(kk-1) + 6);%*** + + omckk_error = solution_error(15+6*(kk-1) + 1:15+6*(kk-1) + 3); + Tckk_error = solution_error(15+6*(kk-1) + 4:15+6*(kk-1) + 6); + + Rckk = rodrigues(omckk); + + Hkk = KK * [Rckk(:,1) Rckk(:,2) Tckk]; + + Hkk = Hkk / Hkk(3,3); + + else + + omckk = NaN*ones(3,1); + Tckk = NaN*ones(3,1); + Rckk = NaN*ones(3,3); + Hkk = NaN*ones(3,3); + omckk_error = NaN*ones(3,1); + Tckk_error = NaN*ones(3,1); + + end; + + eval(['omc_' num2str(kk) ' = omckk;']); + eval(['Rc_' num2str(kk) ' = Rckk;']); + eval(['Tc_' num2str(kk) ' = Tckk;']); + eval(['H_' num2str(kk) '= Hkk;']); + eval(['omc_error_' num2str(kk) ' = omckk_error;']); + eval(['Tc_error_' num2str(kk) ' = Tckk_error;']); + +end; diff --git a/src/g_calib/extract_parameters3D.m b/src/g_calib/extract_parameters3D.m new file mode 100755 index 0000000000000000000000000000000000000000..841c6ab89a78f6ee0729b40b12c6f0f9874f3470 --- /dev/null +++ b/src/g_calib/extract_parameters3D.m @@ -0,0 +1,36 @@ + +%%% Extraction of the final intrinsic and extrinsic paramaters: + + +fc = solution(1:2); +kc = solution(3:6); +cc = solution(6*n_ima + 4 +3:6*n_ima + 5 +3); + +% Calibration matrix: + +KK = [fc(1) 0 cc(1);0 fc(2) cc(2); 0 0 1]; +inv_KK = inv(KK); + +% Extract the extrinsic paramters, and recomputer the collineations + +for kk = 1:n_ima, + + omckk = solution(4+6*(kk-1) + 3:6*kk + 3); + + Tckk = solution(6*kk+1 + 3:6*kk+3 + 3); + + Rckk = rodrigues(omckk); + + Hlkk = KK * [Rckk(:,1) Rckk(:,2) Tckk]; + + Hlkk = Hlkk / Hlkk(3,3); + + eval(['omc_' num2str(kk) ' = omckk;']); + eval(['Rc_' num2str(kk) ' = Rckk;']); + eval(['Tc_' num2str(kk) ' = Tckk;']); + + eval(['Hl_' num2str(kk) '=Hlkk;']); + +end; + + diff --git a/src/g_calib/extract_parameters_fisheye.m b/src/g_calib/extract_parameters_fisheye.m new file mode 100755 index 0000000000000000000000000000000000000000..219be8629718f74592c46c8c6d5cec39953e6adf --- /dev/null +++ b/src/g_calib/extract_parameters_fisheye.m @@ -0,0 +1,61 @@ + +%%% Extraction of the final intrinsic and extrinsic paramaters: + +check_active_images; + +if ~exist('solution_error') + solution_error = zeros(6*n_ima + 15,1); +end; + +fc = solution(1:2);%*** +cc = solution(3:4);%*** +alpha_c = solution(5);%*** +kc = solution(6:9);%*** + +fc_error = solution_error(1:2); +cc_error = solution_error(3:4); +alpha_c_error = solution_error(5); +kc_error = solution_error(6:9); + +% Calibration matrix: + +KK = [fc(1) fc(1)*alpha_c cc(1);0 fc(2) cc(2); 0 0 1]; +inv_KK = inv(KK); + +% Extract the extrinsic paramters, and recomputer the collineations + +for kk = 1:n_ima, + + if active_images(kk), + + omckk = solution(15+6*(kk-1) + 1:15+6*(kk-1) + 3);%*** + Tckk = solution(15+6*(kk-1) + 4:15+6*(kk-1) + 6);%*** + + omckk_error = solution_error(15+6*(kk-1) + 1:15+6*(kk-1) + 3); + Tckk_error = solution_error(15+6*(kk-1) + 4:15+6*(kk-1) + 6); + + Rckk = rodrigues(omckk); + + Hkk = KK * [Rckk(:,1) Rckk(:,2) Tckk]; + + Hkk = Hkk / Hkk(3,3); + + else + + omckk = NaN*ones(3,1); + Tckk = NaN*ones(3,1); + Rckk = NaN*ones(3,3); + Hkk = NaN*ones(3,3); + omckk_error = NaN*ones(3,1); + Tckk_error = NaN*ones(3,1); + + end; + + eval(['omc_' num2str(kk) ' = omckk;']); + eval(['Rc_' num2str(kk) ' = Rckk;']); + eval(['Tc_' num2str(kk) ' = Tckk;']); + eval(['H_' num2str(kk) '= Hkk;']); + eval(['omc_error_' num2str(kk) ' = omckk_error;']); + eval(['Tc_error_' num2str(kk) ' = Tckk_error;']); + +end; diff --git a/src/g_calib/extrinsic_computation.m b/src/g_calib/extrinsic_computation.m new file mode 100755 index 0000000000000000000000000000000000000000..84691b05cd747375012e6f2e76c11759d462e6b4 --- /dev/null +++ b/src/g_calib/extrinsic_computation.m @@ -0,0 +1,186 @@ +%%% INPUT THE IMAGE FILE NAME: + +if ~exist('fc')|~exist('cc')|~exist('kc')|~exist('alpha_c'), + fprintf(1,'No intrinsic camera parameters available.\n'); + return; +end; + +dir; + +fprintf(1,'\n'); +disp('Computation of the extrinsic parameters from an image of a pattern'); +disp('The intrinsic camera parameters are assumed to be known (previously computed)'); + +fprintf(1,'\n'); +image_name = input('Image name (full name without extension): ','s'); + +format_image2 = '0'; + +while format_image2 == '0', + + format_image2 = input('Image format: ([]=''r''=''ras'', ''b''=''bmp'', ''t''=''tif'', ''p''=''pgm'', ''j''=''jpg'', ''m''=''ppm'') ','s'); + + if isempty(format_image2), + format_image2 = 'ras'; + end; + + if lower(format_image2(1)) == 'm', + format_image2 = 'ppm'; + else + if lower(format_image2(1)) == 'b', + format_image2 = 'bmp'; + else + if lower(format_image2(1)) == 't', + format_image2 = 'tif'; + else + if lower(format_image2(1)) == 'p', + format_image2 = 'pgm'; + else + if lower(format_image2(1)) == 'j', + format_image2 = 'jpg'; + else + if lower(format_image2(1)) == 'r', + format_image2 = 'ras'; + else + disp('Invalid image format'); + format_image2 = '0'; % Ask for format once again + end; + end; + end; + end; + end; + end; +end; + +ima_name = [image_name '.' format_image2]; + + +%%% READ IN IMAGE: + +if format_image2(1) == 'p', + if format_image2(2) == 'p', + I = double(loadppm(ima_name)); + else + I = double(loadpgm(ima_name)); + end; +else + if format_image2(1) == 'r', + I = readras(ima_name); + else + I = double(imread(ima_name)); + end; +end; + +if size(I,3)>1, + I = I(:,:,2); +end; + + +%%% EXTRACT GRID CORNERS: + +fprintf(1,'\nExtraction of the grid corners on the image\n'); + +disp('Window size for corner finder (wintx and winty):'); +wintx = input('wintx ([] = 5) = '); +if isempty(wintx), wintx = 5; end; +wintx = round(wintx); +winty = input('winty ([] = 5) = '); +if isempty(winty), winty = 5; end; +winty = round(winty); + +fprintf(1,'Window size = %dx%d\n',2*wintx+1,2*winty+1); + + +[x_ext,X_ext,n_sq_x,n_sq_y,ind_orig,ind_x,ind_y] = extract_grid(I,wintx,winty,fc,cc,kc); + + + +%%% Computation of the Extrinsic Parameters attached to the grid: + +[omc_ext,Tc_ext,Rc_ext,H_ext] = compute_extrinsic(x_ext,X_ext,fc,cc,kc,alpha_c); + + +%%% Reproject the points on the image: + +[x_reproj] = project_points2(X_ext,omc_ext,Tc_ext,fc,cc,kc,alpha_c); + +err_reproj = x_ext - x_reproj; + +err_std2 = std(err_reproj')'; + + +Basis = [X_ext(:,[ind_orig ind_x ind_orig ind_y ind_orig ])]; + +VX = Basis(:,2) - Basis(:,1); +VY = Basis(:,4) - Basis(:,1); + +nX = norm(VX); +nY = norm(VY); + +VZ = min(nX,nY) * cross(VX/nX,VY/nY); + +Basis = [Basis VZ]; + +[x_basis] = project_points2(Basis,omc_ext,Tc_ext,fc,cc,kc,alpha_c); + +dxpos = (x_basis(:,2) + x_basis(:,1))/2; +dypos = (x_basis(:,4) + x_basis(:,3))/2; +dzpos = (x_basis(:,6) + x_basis(:,5))/2; + + + +figure(2); +image(I); +colormap(gray(256)); +hold on; +plot(x_ext(1,:)+1,x_ext(2,:)+1,'r+'); +plot(x_reproj(1,:)+1,x_reproj(2,:)+1,'yo'); +h = text(x_ext(1,ind_orig)-25,x_ext(2,ind_orig)-25,'O'); +set(h,'Color','g','FontSize',14); +h2 = text(dxpos(1)+1,dxpos(2)-30,'X'); +set(h2,'Color','g','FontSize',14); +h3 = text(dypos(1)-30,dypos(2)+1,'Y'); +set(h3,'Color','g','FontSize',14); +h4 = text(dzpos(1)-10,dzpos(2)-20,'Z'); +set(h4,'Color','g','FontSize',14); +plot(x_basis(1,:)+1,x_basis(2,:)+1,'g-','linewidth',2); +title('Image points (+) and reprojected grid points (o)'); +hold off; + + +fprintf(1,'\n\nExtrinsic parameters:\n\n'); +fprintf(1,'Translation vector: Tc_ext = [ %3.6f \t %3.6f \t %3.6f ]\n',Tc_ext); +fprintf(1,'Rotation vector: omc_ext = [ %3.6f \t %3.6f \t %3.6f ]\n',omc_ext); +fprintf(1,'Rotation matrix: Rc_ext = [ %3.6f \t %3.6f \t %3.6f\n',Rc_ext(1,:)'); +fprintf(1,' %3.6f \t %3.6f \t %3.6f\n',Rc_ext(2,:)'); +fprintf(1,' %3.6f \t %3.6f \t %3.6f ]\n',Rc_ext(3,:)'); +fprintf(1,'Pixel error: err = [ %3.5f \t %3.5f ]\n\n',err_std2); + + + + + +return; + + +% Stores the results: + +kk = 1; + +% Stores location of grid wrt camera: + +eval(['omc_' num2str(kk) ' = omc_ext;']); +eval(['Tc_' num2str(kk) ' = Tc_ext;']); + +% Stores the projected points: + +eval(['y_' num2str(kk) ' = x_reproj;']); +eval(['X_' num2str(kk) ' = X_ext;']); +eval(['x_' num2str(kk) ' = x_ext;']); + + +% Organize the points in a grid: + +eval(['n_sq_x_' num2str(kk) ' = n_sq_x;']); +eval(['n_sq_y_' num2str(kk) ' = n_sq_y;']); + \ No newline at end of file diff --git a/src/g_calib/fixallvariables.m b/src/g_calib/fixallvariables.m new file mode 100755 index 0000000000000000000000000000000000000000..27bd31d72a8685140cf03e845351b8bca6b438ca --- /dev/null +++ b/src/g_calib/fixallvariables.m @@ -0,0 +1,19 @@ +% Code that clears all empty or NaN variables + +varlist = whos; + +if ~isempty(varlist), + + Nvar = size(varlist,1); + + for c_var = 1:Nvar, + + var2fix = varlist(c_var).name; + + fixvariable; + + end; + +end; + +clear varlist var2fix Nvar c_var \ No newline at end of file diff --git a/src/g_calib/fixvariable.m b/src/g_calib/fixvariable.m new file mode 100755 index 0000000000000000000000000000000000000000..1118533033c37a23d15082666809018e17fb1a9f --- /dev/null +++ b/src/g_calib/fixvariable.m @@ -0,0 +1,18 @@ +% Code that clears an empty variable, or a NaN vsriable. +% Does not clear structures, or cells. + +if exist('var2fix')==1, + if eval(['exist(''' var2fix ''') == 1']), + if eval(['isempty(' var2fix ')']), + eval(['clear ' var2fix ]); + else + if eval(['~isstruct(' var2fix ')']), + if eval(['~iscell(' var2fix ')']), + if eval(['isnan(' var2fix '(1))']), + eval(['clear ' var2fix ]); + end; + end; + end; + end; + end; +end; diff --git a/src/g_calib/fov.m b/src/g_calib/fov.m new file mode 100755 index 0000000000000000000000000000000000000000..425a1e0df3d7266a4733ac9c72b3b979bb5ae21d --- /dev/null +++ b/src/g_calib/fov.m @@ -0,0 +1,12 @@ +% small program that computes the field of view of a camera (in degrees) + +if ~exist('fc')|~exist('cc')|~exist('nx')|~exist('ny'), + error('Need calibration results to compute FOV (fc,cc,Wcal,Hcal)'); +end; + +FOV_HOR = 180 * ( atan((nx - (cc(1)+.5))/fc(1)) + atan((cc(1)+.5)/fc(1)) )/pi; + +FOV_VER = 180 * ( atan((ny - (cc(2)+.5))/fc(2)) + atan((cc(2)+.5)/fc(2)) )/pi; + +fprintf(1,'Horizontal field of view = %.2f degrees\n',FOV_HOR); +fprintf(1,'Vertical field of view = %.2f degrees\n',FOV_VER); \ No newline at end of file diff --git a/src/g_calib/ginput2.m b/src/g_calib/ginput2.m new file mode 100755 index 0000000000000000000000000000000000000000..e7fa17a99ce74a00f3475dfe9ba2478a855d7a99 --- /dev/null +++ b/src/g_calib/ginput2.m @@ -0,0 +1,207 @@ +function [out1,out2,out3] = ginput2(arg1) +%GINPUT Graphical input from mouse. +% [X,Y] = GINPUT(N) gets N points from the current axes and returns +% the X- and Y-coordinates in length N vectors X and Y. The cursor +% can be positioned using a mouse (or by using the Arrow Keys on some +% systems). Data points are entered by pressing a mouse button +% or any key on the keyboard except carriage return, which terminates +% the input before N points are entered. +% +% [X,Y] = GINPUT gathers an unlimited number of points until the +% return key is pressed. +% +% [X,Y,BUTTON] = GINPUT(N) returns a third result, BUTTON, that +% contains a vector of integers specifying which mouse button was +% used (1,2,3 from left) or ASCII numbers if a key on the keyboard +% was used. + +% Copyright (c) 1984-96 by The MathWorks, Inc. +% $Revision: 5.18 $ $Date: 1996/11/10 17:48:08 $ + +% Fixed version by Jean-Yves Bouguet to have a cross instead of 2 lines +% More visible for images + +out1 = []; out2 = []; out3 = []; y = []; +c = computer; +if ~strcmp(c(1:2),'PC') & ~strcmp(c(1:2),'MA') + tp = get(0,'TerminalProtocol'); +else + tp = 'micro'; +end + +if ~strcmp(tp,'none') & ~strcmp(tp,'x') & ~strcmp(tp,'micro'), + if nargout == 1, + if nargin == 1, + eval('out1 = trmginput(arg1);'); + else + eval('out1 = trmginput;'); + end + elseif nargout == 2 | nargout == 0, + if nargin == 1, + eval('[out1,out2] = trmginput(arg1);'); + else + eval('[out1,out2] = trmginput;'); + end + if nargout == 0 + out1 = [ out1 out2 ]; + end + elseif nargout == 3, + if nargin == 1, + eval('[out1,out2,out3] = trmginput(arg1);'); + else + eval('[out1,out2,out3] = trmginput;'); + end + end +else + + fig = gcf; + figure(gcf); + + if nargin == 0 + how_many = -1; + b = []; + else + how_many = arg1; + b = []; + if isstr(how_many) ... + | size(how_many,1) ~= 1 | size(how_many,2) ~= 1 ... + | ~(fix(how_many) == how_many) ... + | how_many < 0 + error('Requires a positive integer.') + end + if how_many == 0 + ptr_fig = 0; + while(ptr_fig ~= fig) + ptr_fig = get(0,'PointerWindow'); + end + scrn_pt = get(0,'PointerLocation'); + loc = get(fig,'Position'); + pt = [scrn_pt(1) - loc(1), scrn_pt(2) - loc(2)]; + out1 = pt(1); y = pt(2); + elseif how_many < 0 + error('Argument must be a positive integer.') + end + end + + pointer = get(gcf,'pointer'); + set(gcf,'pointer','crosshair'); + fig_units = get(fig,'units'); + char = 0; + + while how_many ~= 0 + % Use no-side effect WAITFORBUTTONPRESS + waserr = 0; + eval('keydown = wfbp;', 'waserr = 1;'); + if(waserr == 1) + if(ishandle(fig)) + set(fig,'pointer',pointer,'units',fig_units); + error('Interrupted'); + else + error('Interrupted by figure deletion'); + end + end + + ptr_fig = get(0,'CurrentFigure'); + if(ptr_fig == fig) + if keydown + char = get(fig, 'CurrentCharacter'); + button = abs(get(fig, 'CurrentCharacter')); + scrn_pt = get(0, 'PointerLocation'); + set(fig,'units','pixels') + loc = get(fig, 'Position'); + pt = [scrn_pt(1) - loc(1), scrn_pt(2) - loc(2)]; + set(fig,'CurrentPoint',pt); + else + button = get(fig, 'SelectionType'); + if strcmp(button,'open') + button = 1; %b(length(b)); + elseif strcmp(button,'normal') + button = 1; + elseif strcmp(button,'extend') + button = 2; + elseif strcmp(button,'alt') + button = 3; + else + error('Invalid mouse selection.') + end + end + pt = get(gca, 'CurrentPoint'); + + how_many = how_many - 1; + + if(char == 13) % & how_many ~= 0) + % if the return key was pressed, char will == 13, + % and that's our signal to break out of here whether + % or not we have collected all the requested data + % points. + % If this was an early breakout, don't include + % the key info in the return arrays. + % We will no longer count it if it's the last input. + break; + end + + out1 = [out1;pt(1,1)]; + y = [y;pt(1,2)]; + b = [b;button]; + end + end + + set(fig,'pointer',pointer,'units',fig_units); + + if nargout > 1 + out2 = y; + if nargout > 2 + out3 = b; + end + else + out1 = [out1 y]; + end + +end + +%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% +function key = wfbp +%WFBP Replacement for WAITFORBUTTONPRESS that has no side effects. + +% Remove figure button functions +fprops = {'windowbuttonupfcn','buttondownfcn', ... + 'windowbuttondownfcn','windowbuttonmotionfcn'}; +fig = gcf; +fvals = get(fig,fprops); +set(fig,fprops,{'','','',''}) + +% Remove all other buttondown functions +ax = findobj(fig,'type','axes'); +if isempty(ax) + ch = {}; +else + ch = get(ax,{'Children'}); +end +for i=1:length(ch), + ch{i} = ch{i}(:)'; +end +h = [ax(:)',ch{:}]; +vals = get(h,{'buttondownfcn'}); +mt = repmat({''},size(vals)); +set(h,{'buttondownfcn'},mt); + +% Now wait for that buttonpress, and check for error conditions +waserr = 0; +eval(['if nargout==0,', ... + ' waitforbuttonpress,', ... + 'else,', ... + ' keydown = waitforbuttonpress;',... + 'end' ], 'waserr = 1;'); + +% Put everything back +if(ishandle(fig)) + set(fig,fprops,fvals) + set(h,{'buttondownfcn'},vals) +end + +if(waserr == 1) + error('Interrupted'); +end + +if nargout>0, key = keydown; end +%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% diff --git a/src/g_calib/ginput3.m b/src/g_calib/ginput3.m new file mode 100755 index 0000000000000000000000000000000000000000..a0b54f26a9f08a5ee1a83449dcdad3c508660738 --- /dev/null +++ b/src/g_calib/ginput3.m @@ -0,0 +1,216 @@ +function [out1,out2,out3] = ginput2(arg1) +%GINPUT Graphical input from mouse. +% [X,Y] = GINPUT(N) gets N points from the current axes and returns +% the X- and Y-coordinates in length N vectors X and Y. The cursor +% can be positioned using a mouse (or by using the Arrow Keys on some +% systems). Data points are entered by pressing a mouse button +% or any key on the keyboard except carriage return, which terminates +% the input before N points are entered. +% +% [X,Y] = GINPUT gathers an unlimited number of points until the +% return key is pressed. +% +% [X,Y,BUTTON] = GINPUT(N) returns a third result, BUTTON, that +% contains a vector of integers specifying which mouse button was +% used (1,2,3 from left) or ASCII numbers if a key on the keyboard +% was used. + +% Copyright (c) 1984-96 by The MathWorks, Inc. +% $Revision: 5.18 $ $Date: 1996/11/10 17:48:08 $ + +% Fixed version by Jean-Yves Bouguet to have a cross instead of 2 lines +% More visible for images + +P = NaN*ones(16,16); +P(1:15,1:15) = 2*ones(15,15); +P(2:14,2:14) = ones(13,13); +P(3:13,3:13) = NaN*ones(11,11); +P(6:10,6:10) = 2*ones(5,5); +P(7:9,7:9) = 1*ones(3,3); + +out1 = []; out2 = []; out3 = []; y = []; +c = computer; +if ~strcmp(c(1:2),'PC') & ~strcmp(c(1:2),'MA') + tp = get(0,'TerminalProtocol'); +else + tp = 'micro'; +end + +if ~strcmp(tp,'none') & ~strcmp(tp,'x') & ~strcmp(tp,'micro'), + if nargout == 1, + if nargin == 1, + eval('out1 = trmginput(arg1);'); + else + eval('out1 = trmginput;'); + end + elseif nargout == 2 | nargout == 0, + if nargin == 1, + eval('[out1,out2] = trmginput(arg1);'); + else + eval('[out1,out2] = trmginput;'); + end + if nargout == 0 + out1 = [ out1 out2 ]; + end + elseif nargout == 3, + if nargin == 1, + eval('[out1,out2,out3] = trmginput(arg1);'); + else + eval('[out1,out2,out3] = trmginput;'); + end + end +else + + fig = gcf; + figure(gcf); + + if nargin == 0 + how_many = -1; + b = []; + else + how_many = arg1; + b = []; + if isstr(how_many) ... + | size(how_many,1) ~= 1 | size(how_many,2) ~= 1 ... + | ~(fix(how_many) == how_many) ... + | how_many < 0 + error('Requires a positive integer.') + end + if how_many == 0 + ptr_fig = 0; + while(ptr_fig ~= fig) + ptr_fig = get(0,'PointerWindow'); + end + scrn_pt = get(0,'PointerLocation'); + loc = get(fig,'Position'); + pt = [scrn_pt(1) - loc(1), scrn_pt(2) - loc(2)]; + out1 = pt(1); y = pt(2); + elseif how_many < 0 + error('Argument must be a positive integer.') + end + end + +pointer = get(gcf,'pointer'); + +set(gcf,'Pointer','custom','PointerShapeCData',P,'PointerShapeHotSpot',[8,8]); +%set(gcf,'pointer','crosshair'); + fig_units = get(fig,'units'); + char = 0; + + while how_many ~= 0 + % Use no-side effect WAITFORBUTTONPRESS + waserr = 0; + eval('keydown = wfbp;', 'waserr = 1;'); + if(waserr == 1) + if(ishandle(fig)) + set(fig,'pointer',pointer,'units',fig_units); + error('Interrupted'); + else + error('Interrupted by figure deletion'); + end + end + + ptr_fig = get(0,'CurrentFigure'); + if(ptr_fig == fig) + if keydown + char = get(fig, 'CurrentCharacter'); + button = abs(get(fig, 'CurrentCharacter')); + scrn_pt = get(0, 'PointerLocation'); + set(fig,'units','pixels') + loc = get(fig, 'Position'); + pt = [scrn_pt(1) - loc(1), scrn_pt(2) - loc(2)]; + set(fig,'CurrentPoint',pt); + else + button = get(fig, 'SelectionType'); + if strcmp(button,'open') + button = 1; %b(length(b)); + elseif strcmp(button,'normal') + button = 1; + elseif strcmp(button,'extend') + button = 2; + elseif strcmp(button,'alt') + button = 3; + else + error('Invalid mouse selection.') + end + end + pt = get(gca, 'CurrentPoint'); + + how_many = how_many - 1; + + if(char == 13) % & how_many ~= 0) + % if the return key was pressed, char will == 13, + % and that's our signal to break out of here whether + % or not we have collected all the requested data + % points. + % If this was an early breakout, don't include + % the key info in the return arrays. + % We will no longer count it if it's the last input. + break; + end + + out1 = [out1;pt(1,1)]; + y = [y;pt(1,2)]; + b = [b;button]; + end + end + + set(fig,'pointer',pointer,'units',fig_units); + + if nargout > 1 + out2 = y; + if nargout > 2 + out3 = b; + end + else + out1 = [out1 y]; + end + +end + +%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% +function key = wfbp +%WFBP Replacement for WAITFORBUTTONPRESS that has no side effects. + +% Remove figure button functions +fprops = {'windowbuttonupfcn','buttondownfcn', ... + 'windowbuttondownfcn','windowbuttonmotionfcn'}; +fig = gcf; +fvals = get(fig,fprops); +set(fig,fprops,{'','','',''}) + +% Remove all other buttondown functions +ax = findobj(fig,'type','axes'); +if isempty(ax) + ch = {}; +else + ch = get(ax,{'Children'}); +end +for i=1:length(ch), + ch{i} = ch{i}(:)'; +end +h = [ax(:)',ch{:}]; +vals = get(h,{'buttondownfcn'}); +mt = repmat({''},size(vals)); +set(h,{'buttondownfcn'},mt); + +% Now wait for that buttonpress, and check for error conditions +waserr = 0; +eval(['if nargout==0,', ... + ' waitforbuttonpress,', ... + 'else,', ... + ' keydown = waitforbuttonpress;',... + 'end' ], 'waserr = 1;'); + +% Put everything back +if(ishandle(fig)) + set(fig,fprops,fvals) + set(h,{'buttondownfcn'},vals) +end + +if(waserr == 1) + error('Interrupted'); +end + +if nargout>0, key = keydown; end +%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% diff --git a/src/g_calib/ginput4.m b/src/g_calib/ginput4.m new file mode 100755 index 0000000000000000000000000000000000000000..9a7c14f0a6ccf1ca2445e737de5531daa9254b9f --- /dev/null +++ b/src/g_calib/ginput4.m @@ -0,0 +1,242 @@ +function [out1,out2,out3] = ginput4(arg1) + +[out1,out2,out3] = ginput(arg1); + +return; + + + +%GINPUT Graphical input from mouse. +% [X,Y] = GINPUT(N) gets N points from the current axes and returns +% the X- and Y-coordinates in length N vectors X and Y. The cursor +% can be positioned using a mouse (or by using the Arrow Keys on some +% systems). Data points are entered by pressing a mouse button +% or any key on the keyboard except carriage return, which terminates +% the input before N points are entered. +% +% [X,Y] = GINPUT gathers an unlimited number of points until the +% return key is pressed. +% +% [X,Y,BUTTON] = GINPUT(N) returns a third result, BUTTON, that +% contains a vector of integers specifying which mouse button was +% used (1,2,3 from left) or ASCII numbers if a key on the keyboard +% was used. +% +% Examples: +% [x,y] = ginput; +% +% [x,y] = ginput(5); +% +% [x, y, button] = ginput(1); +% +% See also GTEXT, UIRESTORE, UISUSPEND, WAITFORBUTTONPRESS. + +% Copyright 1984-2006 The MathWorks, Inc. +% $Revision: 5.32.4.9 $ $Date: 2006/12/20 07:19:10 $ + + +P = NaN*ones(16,16); +P(1:15,1:15) = 2*ones(15,15); +P(2:14,2:14) = ones(13,13); +P(3:13,3:13) = NaN*ones(11,11); +P(6:10,6:10) = 2*ones(5,5); +P(7:9,7:9) = 1*ones(3,3); + + +out1 = []; out2 = []; out3 = []; y = []; +c = computer; +if ~strcmp(c(1:2),'PC') + tp = get(0,'TerminalProtocol'); +else + tp = 'micro'; +end + +if ~strcmp(tp,'none') && ~strcmp(tp,'x') && ~strcmp(tp,'micro'), + if nargout == 1, + if nargin == 1, + out1 = trmginput(arg1); + else + out1 = trmginput; + end + elseif nargout == 2 || nargout == 0, + if nargin == 1, + [out1,out2] = trmginput(arg1); + else + [out1,out2] = trmginput; + end + if nargout == 0 + out1 = [ out1 out2 ]; + end + elseif nargout == 3, + if nargin == 1, + [out1,out2,out3] = trmginput(arg1); + else + [out1,out2,out3] = trmginput; + end + end +else + + fig = gcf; + figure(gcf); + + if nargin == 0 + how_many = -1; + b = []; + else + how_many = arg1; + b = []; + if ischar(how_many) ... + || size(how_many,1) ~= 1 || size(how_many,2) ~= 1 ... + || ~(fix(how_many) == how_many) ... + || how_many < 0 + error('MATLAB:ginput:NeedPositiveInt', 'Requires a positive integer.') + end + if how_many == 0 + ptr_fig = 0; + while(ptr_fig ~= fig) + ptr_fig = get(0,'PointerWindow'); + end + scrn_pt = get(0,'PointerLocation'); + loc = get(fig,'Position'); + pt = [scrn_pt(1) - loc(1), scrn_pt(2) - loc(2)]; + out1 = pt(1); y = pt(2); + elseif how_many < 0 + error('MATLAB:ginput:InvalidArgument', 'Argument must be a positive integer.') + end + end + + % Suspend figure functions + state = uisuspend(fig); + + toolbar = findobj(allchild(fig),'flat','Type','uitoolbar'); + if ~isempty(toolbar) + ptButtons = [uigettool(toolbar,'Plottools.PlottoolsOff'), ... + uigettool(toolbar,'Plottools.PlottoolsOn')]; + ptState = get (ptButtons,'Enable'); + set (ptButtons,'Enable','off'); + end + + %set(fig,'pointer','fullcrosshair'); + set(fig,'Pointer','custom','PointerShapeCData',P,'PointerShapeHotSpot',[8,8]); + + fig_units = get(fig,'units'); + char = 0; + + % We need to pump the event queue on unix + % before calling WAITFORBUTTONPRESS + drawnow + + while how_many ~= 0 + % Use no-side effect WAITFORBUTTONPRESS + waserr = 0; + try + keydown = wfbp; + catch + waserr = 1; + end + if(waserr == 1) + if(ishandle(fig)) + set(fig,'units',fig_units); + uirestore(state); + error('MATLAB:ginput:Interrupted', 'Interrupted'); + else + error('MATLAB:ginput:FigureDeletionPause', 'Interrupted by figure deletion'); + end + end + + ptr_fig = get(0,'CurrentFigure'); + if(ptr_fig == fig) + if keydown + char = get(fig, 'CurrentCharacter'); + button = abs(get(fig, 'CurrentCharacter')); + scrn_pt = get(0, 'PointerLocation'); + set(fig,'units','pixels') + loc = get(fig, 'Position'); + % We need to compensate for an off-by-one error: + pt = [scrn_pt(1) - loc(1) + 1, scrn_pt(2) - loc(2) + 1]; + set(fig,'CurrentPoint',pt); + else + button = get(fig, 'SelectionType'); + if strcmp(button,'open') + button = 1; + elseif strcmp(button,'normal') + button = 1; + elseif strcmp(button,'extend') + button = 2; + elseif strcmp(button,'alt') + button = 3; + else + error('MATLAB:ginput:InvalidSelection', 'Invalid mouse selection.') + end + end + pt = get(gca, 'CurrentPoint'); + + how_many = how_many - 1; + + if(char == 13) % & how_many ~= 0) + % if the return key was pressed, char will == 13, + % and that's our signal to break out of here whether + % or not we have collected all the requested data + % points. + % If this was an early breakout, don't include + % the key info in the return arrays. + % We will no longer count it if it's the last input. + break; + end + + out1 = [out1;pt(1,1)]; + y = [y;pt(1,2)]; + b = [b;button]; + end + end + + uirestore(state); + if ~isempty(toolbar) && ~isempty(ptButtons) + set (ptButtons(1),'Enable',ptState{1}); + set (ptButtons(2),'Enable',ptState{2}); + end + set(fig,'units',fig_units); + + if nargout > 1 + out2 = y; + if nargout > 2 + out3 = b; + end + else + out1 = [out1 y]; + end + +end + +%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% +function key = wfbp +%WFBP Replacement for WAITFORBUTTONPRESS that has no side effects. + +fig = gcf; +current_char = []; + +% Now wait for that buttonpress, and check for error conditions +waserr = 0; +try + h=findall(fig,'type','uimenu','accel','C'); % Disabling ^C for edit menu so the only ^C is for + set(h,'accel',''); % interrupting the function. + keydown = waitforbuttonpress; + current_char = double(get(fig,'CurrentCharacter')); % Capturing the character. + if~isempty(current_char) && (keydown == 1) % If the character was generated by the + if(current_char == 3) % current keypress AND is ^C, set 'waserr'to 1 + waserr = 1; % so that it errors out. + end + end + + set(h,'accel','C'); % Set back the accelerator for edit menu. +catch + waserr = 1; +end +drawnow; +if(waserr == 1) + set(h,'accel','C'); % Set back the accelerator if it errored out. + error('MATLAB:ginput:Interrupted', 'Interrupted'); +end + +if nargout>0, key = keydown; end +%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% diff --git a/src/g_calib/go_calib_optim.m b/src/g_calib/go_calib_optim.m new file mode 100755 index 0000000000000000000000000000000000000000..6c51d28f31979fcf1b05c46ca10bcae293ca1c12 --- /dev/null +++ b/src/g_calib/go_calib_optim.m @@ -0,0 +1,68 @@ +%go_calib_optim +% +%Main calibration function. Computes the intrinsic andextrinsic parameters. +%Runs as a script. +% +%INPUT: x_1,x_2,x_3,...: Feature locations on the images +% X_1,X_2,X_3,...: Corresponding grid coordinates +% +%OUTPUT: fc: Camera focal length +% cc: Principal point coordinates +% alpha_c: Skew coefficient +% kc: Distortion coefficients +% KK: The camera matrix (containing fc and cc) +% omc_1,omc_2,omc_3,...: 3D rotation vectors attached to the grid positions in space +% Tc_1,Tc_2,Tc_3,...: 3D translation vectors attached to the grid positions in space +% Rc_1,Rc_2,Rc_3,...: 3D rotation matrices corresponding to the omc vectors +% +%Method: Minimizes the pixel reprojection error in the least squares sense over the intrinsic +% camera parameters, and the extrinsic parameters (3D locations of the grids in space) +% +%Note: If the intrinsic camera parameters (fc, cc, kc) do not exist before, they are initialized through +% the function init_intrinsic_param.m. Otherwise, the variables in memory are used as initial guesses. +% +%Note: The row vector active_images consists of zeros and ones. To deactivate an image, set the +% corresponding entry in the active_images vector to zero. +% +%VERY IMPORTANT: This function works for 2D and 3D calibration rigs, except for init_intrinsic_param.m +%that is so far implemented to work only with 2D rigs. +%In the future, a more general function will be there. +%For now, if using a 3D calibration rig, set quick_init to 1 for an easy initialization of the focal length + + +if ~exist('n_ima'), + data_calib; % Load the images + click_calib; % Extract the corners +end; + + +check_active_images; +check_extracted_images; +check_active_images; +desactivated_images = []; + +recompute_extrinsic = (length(ind_active) < 100); % if there are too many images, do not spend time recomputing the extrinsic parameters twice.. + +if ~exist('rosette_calibration', 'var') + rosette_calibration = 0; +end; + +if (rosette_calibration) + %%% Special Setting for the Rosette: + est_dist = ones(5,1); +end; + +%%% MAIN OPTIMIZATION CALL!!!!! (look into this function for the details of implementation) +go_calib_optim_iter; + +if ~isempty(desactivated_images), + param_list_save = param_list; + fprintf(1,'\nNew optimization including the images that have been deactivated during the previous optimization.\n'); + active_images(desactivated_images) = ones(1,length(desactivated_images)); + desactivated_images = []; + go_calib_optim_iter; + if ~isempty(desactivated_images), + fprintf(1,['List of images left desactivated: ' num2str(desactivated_images) '\n' ] ); + end; + param_list = [param_list_save(:,1:end-1) param_list]; +end; diff --git a/src/g_calib/go_calib_optim_fisheye_no_read.m b/src/g_calib/go_calib_optim_fisheye_no_read.m new file mode 100755 index 0000000000000000000000000000000000000000..67fe03fc12d9a55f1c583a0489a812b91cb9095f --- /dev/null +++ b/src/g_calib/go_calib_optim_fisheye_no_read.m @@ -0,0 +1,68 @@ +%go_calib_optim +% +%Main calibration function. Computes the intrinsic andextrinsic parameters. +%Runs as a script. +% +%INPUT: x_1,x_2,x_3,...: Feature locations on the images +% X_1,X_2,X_3,...: Corresponding grid coordinates +% +%OUTPUT: fc: Camera focal length +% cc: Principal point coordinates +% alpha_c: Skew coefficient +% kc: Distortion coefficients +% KK: The camera matrix (containing fc and cc) +% omc_1,omc_2,omc_3,...: 3D rotation vectors attached to the grid positions in space +% Tc_1,Tc_2,Tc_3,...: 3D translation vectors attached to the grid positions in space +% Rc_1,Rc_2,Rc_3,...: 3D rotation matrices corresponding to the omc vectors +% +%Method: Minimizes the pixel reprojection error in the least squares sense over the intrinsic +% camera parameters, and the extrinsic parameters (3D locations of the grids in space) +% +%Note: If the intrinsic camera parameters (fc, cc, kc) do not exist before, they are initialized through +% the function init_intrinsic_param.m. Otherwise, the variables in memory are used as initial guesses. +% +%Note: The row vector active_images consists of zeros and ones. To deactivate an image, set the +% corresponding entry in the active_images vector to zero. +% +%VERY IMPORTANT: This function works for 2D and 3D calibration rigs, except for init_intrinsic_param.m +%that is so far implemented to work only with 2D rigs. +%In the future, a more general function will be there. +%For now, if using a 3D calibration rig, set quick_init to 1 for an easy initialization of the focal length + + +if ~exist('n_ima'), + data_calib_no_read; % Load the images + click_calib_fisheye_no_read; % Extract the corners +end; + + +check_active_images; +check_extracted_images; +check_active_images; +desactivated_images = []; + +recompute_extrinsic = (length(ind_active) < 100); % if there are too many images, do not spend time recomputing the extrinsic parameters twice.. + +if ~exist('rosette_calibration', 'var') + rosette_calibration = 0; +end; + +if (rosette_calibration) + %%% Special Setting for the Rosette: + est_dist = [ones(2,1);zeros(2,1)]; +end; + +%%% MAIN OPTIMIZATION CALL!!!!! (look into this function for the details of implementation) +go_calib_optim_iter_fisheye; + +if ~isempty(desactivated_images), + param_list_save = param_list; + fprintf(1,'\nNew optimization including the images that have been deactivated during the previous optimization.\n'); + active_images(desactivated_images) = ones(1,length(desactivated_images)); + desactivated_images = []; + go_calib_optim_iter_fisheye; + if ~isempty(desactivated_images), + fprintf(1,['List of images left desactivated: ' num2str(desactivated_images) '\n' ] ); + end; + param_list = [param_list_save(:,1:end-1) param_list]; +end; diff --git a/src/g_calib/go_calib_optim_iter.m b/src/g_calib/go_calib_optim_iter.m new file mode 100755 index 0000000000000000000000000000000000000000..c5b386d1ac9d99d10b1ab06f38f1f20d7c69ea4b --- /dev/null +++ b/src/g_calib/go_calib_optim_iter.m @@ -0,0 +1,635 @@ +%go_calib_optim_iter +% +%Main calibration function. Computes the intrinsic andextrinsic parameters. +%Runs as a script. +% +%INPUT: x_1,x_2,x_3,...: Feature locations on the images +% X_1,X_2,X_3,...: Corresponding grid coordinates +% +%OUTPUT: fc: Camera focal length +% cc: Principal point coordinates +% alpha_c: Skew coefficient +% kc: Distortion coefficients +% KK: The camera matrix (containing fc and cc) +% omc_1,omc_2,omc_3,...: 3D rotation vectors attached to the grid positions in space +% Tc_1,Tc_2,Tc_3,...: 3D translation vectors attached to the grid positions in space +% Rc_1,Rc_2,Rc_3,...: 3D rotation matrices corresponding to the omc vectors +% +%Method: Minimizes the pixel reprojection error in the least squares sense over the intrinsic +% camera parameters, and the extrinsic parameters (3D locations of the grids in space) +% +%Note: If the intrinsic camera parameters (fc, cc, kc) do not exist before, they are initialized through +% the function init_intrinsic_param.m. Otherwise, the variables in memory are used as initial guesses. +% +%Note: The row vector active_images consists of zeros and ones. To deactivate an image, set the +% corresponding entry in the active_images vector to zero. +% +%VERY IMPORTANT: This function works for 2D and 3D calibration rigs, except for init_intrinsic_param.m +%that is so far implemented to work only with 2D rigs. +%In the future, a more general function will be there. +%For now, if using a 3D calibration rig, quick_init is set to 1 for an easy initialization of the focal length + +if ~exist('desactivated_images'), + desactivated_images = []; +end; + + + +if ~exist('est_aspect_ratio'), + est_aspect_ratio = 1; +end; + +if ~exist('est_fc'); + est_fc = [1;1]; % Set to zero if you do not want to estimate the focal length (it may be useful! believe it or not!) +end; + +if ~exist('recompute_extrinsic'), + recompute_extrinsic = 1; % Set this variable to 0 in case you do not want to recompute the extrinsic parameters + % at each iterstion. +end; + +if ~exist('MaxIter'), + MaxIter = 30; % Maximum number of iterations in the gradient descent +end; + +if ~exist('check_cond'), + check_cond = 1; % Set this variable to 0 in case you don't want to extract view dynamically +end; + +if ~exist('center_optim'), + center_optim = 1; %%% Set this variable to 0 if your do not want to estimate the principal point +end; + +if exist('est_dist'), + if length(est_dist) == 4, + est_dist = [est_dist ; 0]; + end; +end; + +if ~exist('est_dist'), + est_dist = [1;1;1;1;0]; +end; + +if ~exist('est_alpha'), + est_alpha = 0; % by default, do not estimate skew +end; + + +% Little fix in case of stupid values in the binary variables: +center_optim = double(~~center_optim); +est_alpha = double(~~est_alpha); +est_dist = double(~~est_dist); +est_fc = double(~~est_fc); +est_aspect_ratio = double(~~est_aspect_ratio); + + + +fprintf(1,'\n'); + +if ~exist('nx')&~exist('ny'), + fprintf(1,'WARNING: No image size (nx,ny) available. Setting nx=640 and ny=480. If these are not the right values, change values manually.\n'); + nx = 640; + ny = 480; +end; + + +check_active_images; + + +quick_init = 0; % Set to 1 for using a quick init (necessary when using 3D rigs) + + +% Check 3D-ness of the calibration rig: +rig3D = 0; +for kk = ind_active, + eval(['X_kk = X_' num2str(kk) ';']); + if is3D(X_kk), + rig3D = 1; + end; +end; + + +if center_optim & (length(ind_active) < 2) & ~rig3D, + fprintf(1,'WARNING: Principal point rejected from the optimization when using one image and planar rig (center_optim = 1).\n'); + center_optim = 0; %%% when using a single image, please, no principal point estimation!!! + est_alpha = 0; +end; + +if ~exist('dont_ask'), + dont_ask = 0; +end; + +if center_optim & (length(ind_active) < 5) & ~rig3D, + fprintf(1,'WARNING: The principal point estimation may be unreliable (using less than 5 images for calibration).\n'); + %if ~dont_ask, + % quest = input('Are you sure you want to keep the principal point in the optimization process? ([]=yes, other=no) '); + % center_optim = isempty(quest); + %end; +end; + + +% A quick fix for solving conflict +if ~isequal(est_fc,[1;1]), + est_aspect_ratio=1; +end; +if ~est_aspect_ratio, + est_fc=[1;1]; +end; + + +if ~est_aspect_ratio, + fprintf(1,'Aspect ratio not optimized (est_aspect_ratio = 0) -> fc(1)=fc(2). Set est_aspect_ratio to 1 for estimating aspect ratio.\n'); +else + if isequal(est_fc,[1;1]), + fprintf(1,'Aspect ratio optimized (est_aspect_ratio = 1) -> both components of fc are estimated (DEFAULT).\n'); + end; +end; + +if ~isequal(est_fc,[1;1]), + if isequal(est_fc,[1;0]), + fprintf(1,'The first component of focal (fc(1)) is estimated, but not the second one (est_fc=[1;0])\n'); + else + if isequal(est_fc,[0;1]), + fprintf(1,'The second component of focal (fc(1)) is estimated, but not the first one (est_fc=[0;1])\n'); + else + fprintf(1,'The focal vector fc is not optimized (est_fc=[0;0])\n'); + end; + end; +end; + + +if ~center_optim, % In the case where the principal point is not estimated, keep it at the center of the image + fprintf(1,'Principal point not optimized (center_optim=0). '); + if ~exist('cc'), + fprintf(1,'It is kept at the center of the image.\n'); + cc = [(nx-1)/2;(ny-1)/2]; + else + fprintf(1,'Note: to set it in the middle of the image, clear variable cc, and run calibration again.\n'); + end; +else + fprintf(1,'Principal point optimized (center_optim=1) - (DEFAULT). To reject principal point, set center_optim=0\n'); +end; + + +if ~center_optim & (est_alpha), + fprintf(1,'WARNING: Since there is no principal point estimation (center_optim=0), no skew estimation (est_alpha = 0)\n'); + est_alpha = 0; +end; + +if ~est_alpha, + fprintf(1,'Skew not optimized (est_alpha=0) - (DEFAULT)\n'); + alpha_c = 0; +else + fprintf(1,'Skew optimized (est_alpha=1). To disable skew estimation, set est_alpha=0.\n'); +end; + + +if ~prod(double(est_dist)), + fprintf(1,'Distortion not fully estimated (defined by the variable est_dist):\n'); + if ~est_dist(1), + fprintf(1,' Second order distortion not estimated (est_dist(1)=0).\n'); + end; + if ~est_dist(2), + fprintf(1,' Fourth order distortion not estimated (est_dist(2)=0).\n'); + end; + if ~est_dist(5), + fprintf(1,' Sixth order distortion not estimated (est_dist(5)=0) - (DEFAULT) .\n'); + end; + if ~prod(double(est_dist(3:4))), + fprintf(1,' Tangential distortion not estimated (est_dist(3:4)~=[1;1]).\n'); + end; +end; + + +% Check 3D-ness of the calibration rig: +rig3D = 0; +for kk = ind_active, + eval(['X_kk = X_' num2str(kk) ';']); + if is3D(X_kk), + rig3D = 1; + end; +end; + +% If the rig is 3D, then no choice: the only valid initialization is manual! +if rig3D, + quick_init = 1; +end; + + + +alpha_smooth = 0.1; % set alpha_smooth = 1; for steepest gradient descent + + +% Conditioning threshold for view rejection +thresh_cond = 1e6; + + + +% Initialization of the intrinsic parameters (if necessary) + +if ~exist('cc'), + fprintf(1,'Initialization of the principal point at the center of the image.\n'); + cc = [(nx-1)/2;(ny-1)/2]; + alpha_smooth = 0.1; % slow convergence +end; + + +if exist('kc'), + if length(kc) == 4; + fprintf(1,'Adding a new distortion coefficient to kc -> radial distortion model up to the 6th degree'); + kc = [kc;0]; + end; +end; + + + +if ~exist('alpha_c'), + fprintf(1,'Initialization of the image skew to zero.\n'); + alpha_c = 0; + alpha_smooth = 0.1; % slow convergence +end; + +if ~exist('fc')& quick_init, + FOV_angle = 35; % Initial camera field of view in degrees + fprintf(1,['Initialization of the focal length to a FOV of ' num2str(FOV_angle) ' degrees.\n']); + fc = (nx/2)/tan(pi*FOV_angle/360) * ones(2,1); + est_fc = [1;1]; + alpha_smooth = 0.1; % slow +end; + + +if ~exist('fc'), + % Initialization of the intrinsic parameters: + fprintf(1,'Initialization of the intrinsic parameters using the vanishing points of planar patterns.\n') + init_intrinsic_param; % The right way to go (if quick_init is not active)! + alpha_smooth = 0.1; % slow convergence + est_fc = [1;1]; +end; + + +if ~exist('kc'), + fprintf(1,'Initialization of the image distortion to zero.\n'); + kc = zeros(5,1); + alpha_smooth = 0.1; % slow convergence +end; + +if ~est_aspect_ratio, + fc(1) = (fc(1)+fc(2))/2; + fc(2) = fc(1); +end; + +if ~prod(double(est_dist)), + % If no distortion estimated, set to zero the variables that are not estimated + kc = kc .* est_dist; +end; + + +if ~prod(double(est_fc)), + fprintf(1,'Warning: The focal length is not fully estimated (est_fc ~= [1;1])\n'); +end; + + +%%% Initialization of the extrinsic parameters for global minimization: +comp_ext_calib; + + + +%%% Initialization of the global parameter vector: + +init_param = [fc;cc;alpha_c;kc;zeros(5,1)]; + +for kk = 1:n_ima, + eval(['omckk = omc_' num2str(kk) ';']); + eval(['Tckk = Tc_' num2str(kk) ';']); + init_param = [init_param; omckk ; Tckk]; +end; + + + +%-------------------- Main Optimization: + +fprintf(1,'\nMain calibration optimization procedure - Number of images: %d\n',length(ind_active)); + + +param = init_param; +change = 1; + +iter = 0; + +fprintf(1,'Gradient descent iterations: '); + +param_list = param; + + +while (change > 1e-9)&(iter < MaxIter), + + fprintf(1,'%d...',iter+1); + + % To speed up: pre-allocate the memory for the Jacobian JJ3. + % For that, need to compute the total number of points. + + %% The first step consists of updating the whole vector of knowns (intrinsic + extrinsic of active + %% images) through a one step steepest gradient descent. + + + f = param(1:2); + c = param(3:4); + alpha = param(5); + k = param(6:10); + + + % Compute the size of the Jacobian matrix: + N_points_views_active = N_points_views(ind_active); + + JJ3 = sparse([],[],[],15 + 6*n_ima,15 + 6*n_ima,126*n_ima + 225); + ex3 = zeros(15 + 6*n_ima,1); + + + for kk = ind_active, %1:n_ima, + %if active_images(kk), + + omckk = param(15+6*(kk-1) + 1:15+6*(kk-1) + 3); + + Tckk = param(15+6*(kk-1) + 4:15+6*(kk-1) + 6); + + if isnan(omckk(1)), + fprintf(1,'Intrinsic parameters at frame %d do not exist\n',kk); + return; + end; + + eval(['X_kk = X_' num2str(kk) ';']); + eval(['x_kk = x_' num2str(kk) ';']); + + Np = N_points_views(kk); + + if ~est_aspect_ratio, + [x,dxdom,dxdT,dxdf,dxdc,dxdk,dxdalpha] = project_points2(X_kk,omckk,Tckk,f(1),c,k,alpha); + dxdf = repmat(dxdf,[1 2]); + else + [x,dxdom,dxdT,dxdf,dxdc,dxdk,dxdalpha] = project_points2(X_kk,omckk,Tckk,f,c,k,alpha); + end; + + exkk = x_kk - x; + + A = [dxdf dxdc dxdalpha dxdk]'; + B = [dxdom dxdT]'; + + JJ3(1:10,1:10) = JJ3(1:10,1:10) + sparse(A*A'); + JJ3(15+6*(kk-1) + 1:15+6*(kk-1) + 6,15+6*(kk-1) + 1:15+6*(kk-1) + 6) = sparse(B*B'); + + AB = sparse(A*B'); + JJ3(1:10,15+6*(kk-1) + 1:15+6*(kk-1) + 6) = AB; + JJ3(15+6*(kk-1) + 1:15+6*(kk-1) + 6,1:10) = (AB)'; + + ex3(1:10) = ex3(1:10) + A*exkk(:); + ex3(15+6*(kk-1) + 1:15+6*(kk-1) + 6) = B*exkk(:); + + % Check if this view is ill-conditioned: + if check_cond, + JJ_kk = B'; %[dxdom dxdT]; + if (cond(JJ_kk)> thresh_cond), + active_images(kk) = 0; + fprintf(1,'\nWarning: View #%d ill-conditioned. This image is now set inactive. (note: to disactivate this option, set check_cond=0)\n',kk) + desactivated_images = [desactivated_images kk]; + param(15+6*(kk-1) + 1:15+6*(kk-1) + 6) = NaN*ones(6,1); + end; + end; + + %end; + + end; + + + % List of active images (necessary if changed): + check_active_images; + + + % The following vector helps to select the variables to update (for only active images): + selected_variables = [est_fc;center_optim*ones(2,1);est_alpha;est_dist;zeros(5,1);reshape(ones(6,1)*active_images,6*n_ima,1)]; + if ~est_aspect_ratio, + if isequal(est_fc,[1;1]) | isequal(est_fc,[1;0]), + selected_variables(2) = 0; + end; + end; + ind_Jac = find(selected_variables)'; + + JJ3 = JJ3(ind_Jac,ind_Jac); + ex3 = ex3(ind_Jac); + + JJ2_inv = inv(JJ3); % not bad for sparse matrices!! + + + % Smoothing coefficient: + + alpha_smooth2 = 1-(1-alpha_smooth)^(iter+1); %set to 1 to undo any smoothing! + + param_innov = alpha_smooth2*JJ2_inv*ex3; + + + param_up = param(ind_Jac) + param_innov; + param(ind_Jac) = param_up; + + + % New intrinsic parameters: + + fc_current = param(1:2); + cc_current = param(3:4); + + if center_optim & ((param(3)<0)|(param(3)>nx)|(param(4)<0)|(param(4)>ny)), + fprintf(1,'Warning: it appears that the principal point cannot be estimated. Setting center_optim = 0\n'); + center_optim = 0; + cc_current = c; + else + cc_current = param(3:4); + end; + + alpha_current = param(5); + kc_current = param(6:10); + + if ~est_aspect_ratio & isequal(est_fc,[1;1]), + fc_current(2) = fc_current(1); + param(2) = param(1); + end; + + % Change on the intrinsic parameters: + change = norm([fc_current;cc_current] - [f;c])/norm([fc_current;cc_current]); + + + %% Second step: (optional) - It makes convergence faster, and the region of convergence LARGER!!! + %% Recompute the extrinsic parameters only using compute_extrinsic.m (this may be useful sometimes) + %% The complete gradient descent method is useful to precisely update the intrinsic parameters. + + + if recompute_extrinsic, + MaxIter2 = 20; + for kk =ind_active, %1:n_ima, + %if active_images(kk), + omc_current = param(15+6*(kk-1) + 1:15+6*(kk-1) + 3); + Tc_current = param(15+6*(kk-1) + 4:15+6*(kk-1) + 6); + eval(['X_kk = X_' num2str(kk) ';']); + eval(['x_kk = x_' num2str(kk) ';']); + [omc_current,Tc_current] = compute_extrinsic_init(x_kk,X_kk,fc_current,cc_current,kc_current,alpha_current); + [omckk,Tckk,Rckk,JJ_kk] = compute_extrinsic_refine(omc_current,Tc_current,x_kk,X_kk,fc_current,cc_current,kc_current,alpha_current,MaxIter2,thresh_cond); + if check_cond, + if (cond(JJ_kk)> thresh_cond), + active_images(kk) = 0; + fprintf(1,'\nWarning: View #%d ill-conditioned. This image is now set inactive. (note: to disactivate this option, set check_cond=0)\n',kk); + desactivated_images = [desactivated_images kk]; + omckk = NaN*ones(3,1); + Tckk = NaN*ones(3,1); + end; + end; + param(15+6*(kk-1) + 1:15+6*(kk-1) + 3) = omckk; + param(15+6*(kk-1) + 4:15+6*(kk-1) + 6) = Tckk; + %end; + end; + end; + + param_list = [param_list param]; + iter = iter + 1; + +end; + +fprintf(1,'done\n'); + + + +%%%--------------------------- Computation of the error of estimation: + +fprintf(1,'Estimation of uncertainties...'); + + +check_active_images; + +solution = param; + + +% Extraction of the paramters for computing the right reprojection error: + +fc = solution(1:2); +cc = solution(3:4); +alpha_c = solution(5); +kc = solution(6:10); + +for kk = 1:n_ima, + + if active_images(kk), + + omckk = solution(15+6*(kk-1) + 1:15+6*(kk-1) + 3);%*** + Tckk = solution(15+6*(kk-1) + 4:15+6*(kk-1) + 6);%*** + Rckk = rodrigues(omckk); + + else + + omckk = NaN*ones(3,1); + Tckk = NaN*ones(3,1); + Rckk = NaN*ones(3,3); + + end; + + eval(['omc_' num2str(kk) ' = omckk;']); + eval(['Rc_' num2str(kk) ' = Rckk;']); + eval(['Tc_' num2str(kk) ' = Tckk;']); + +end; + + +% Recompute the error (in the vector ex): +comp_error_calib; + +sigma_x = std(ex(:)); + +% Compute the size of the Jacobian matrix: +N_points_views_active = N_points_views(ind_active); + +JJ3 = sparse([],[],[],15 + 6*n_ima,15 + 6*n_ima,126*n_ima + 225); + +for kk = ind_active, + + omckk = param(15+6*(kk-1) + 1:15+6*(kk-1) + 3); + Tckk = param(15+6*(kk-1) + 4:15+6*(kk-1) + 6); + + eval(['X_kk = X_' num2str(kk) ';']); + + Np = N_points_views(kk); + + %[x,dxdom,dxdT,dxdf,dxdc,dxdk,dxdalpha] = project_points2(X_kk,omckk,Tckk,fc,cc,kc,alpha_c); + + if ~est_aspect_ratio, + [x,dxdom,dxdT,dxdf,dxdc,dxdk,dxdalpha] = project_points2(X_kk,omckk,Tckk,fc(1),cc,kc,alpha_c); + dxdf = repmat(dxdf,[1 2]); + else + [x,dxdom,dxdT,dxdf,dxdc,dxdk,dxdalpha] = project_points2(X_kk,omckk,Tckk,fc,cc,kc,alpha_c); + end; + + A = [dxdf dxdc dxdalpha dxdk]'; + B = [dxdom dxdT]'; + + JJ3(1:10,1:10) = JJ3(1:10,1:10) + sparse(A*A'); + JJ3(15+6*(kk-1) + 1:15+6*(kk-1) + 6,15+6*(kk-1) + 1:15+6*(kk-1) + 6) = sparse(B*B'); + + AB = sparse(A*B'); + JJ3(1:10,15+6*(kk-1) + 1:15+6*(kk-1) + 6) = AB; + JJ3(15+6*(kk-1) + 1:15+6*(kk-1) + 6,1:10) = (AB)'; + +end; + +JJ3 = JJ3(ind_Jac,ind_Jac); + +JJ2_inv = inv(JJ3); % not bad for sparse matrices!! + +param_error = zeros(6*n_ima+15,1); +param_error(ind_Jac) = 3*sqrt(full(diag(JJ2_inv)))*sigma_x; + +solution_error = param_error; + +if ~est_aspect_ratio & isequal(est_fc,[1;1]), + solution_error(2) = solution_error(1); +end; + + +%%% Extraction of the final intrinsic and extrinsic paramaters: + +extract_parameters; + +fprintf(1,'done\n'); + + +fprintf(1,'\n\nCalibration results after optimization (with uncertainties):\n\n'); +fprintf(1,'Focal Length: fc = [ %3.5f %3.5f ] ± [ %3.5f %3.5f ]\n',[fc;fc_error]); +fprintf(1,'Principal point: cc = [ %3.5f %3.5f ] ± [ %3.5f %3.5f ]\n',[cc;cc_error]); +fprintf(1,'Skew: alpha_c = [ %3.5f ] ± [ %3.5f ] => angle of pixel axes = %3.5f ± %3.5f degrees\n',[alpha_c;alpha_c_error],90 - atan(alpha_c)*180/pi,atan(alpha_c_error)*180/pi); +fprintf(1,'Distortion: kc = [ %3.5f %3.5f %3.5f %3.5f %5.5f ] ± [ %3.5f %3.5f %3.5f %3.5f %5.5f ]\n',[kc;kc_error]); +fprintf(1,'Pixel error: err = [ %3.5f %3.5f ]\n\n',err_std); +fprintf(1,'Note: The numerical errors are approximately three times the standard deviations (for reference).\n\n\n') +%fprintf(1,' For accurate (and stable) error estimates, it is recommended to run Calibration once again.\n\n\n') + + + +%%% Some recommendations to the user to reject some of the difficult unkowns... Still in debug mode. + +alpha_c_min = alpha_c - alpha_c_error/2; +alpha_c_max = alpha_c + alpha_c_error/2; + +if (alpha_c_min < 0) & (alpha_c_max > 0), + fprintf(1,'Recommendation: The skew coefficient alpha_c is found to be equal to zero (within its uncertainty).\n'); + fprintf(1,' You may want to reject it from the optimization by setting est_alpha=0 and run Calibration\n\n'); +end; + +kc_min = kc - kc_error/2; +kc_max = kc + kc_error/2; + +prob_kc = (kc_min < 0) & (kc_max > 0); + +if ~(prob_kc(3) & prob_kc(4)) + prob_kc(3:4) = [0;0]; +end; + + +if sum(prob_kc), + fprintf(1,'Recommendation: Some distortion coefficients are found equal to zero (within their uncertainties).\n'); + fprintf(1,' To reject them from the optimization set est_dist=[%d;%d;%d;%d;%d] and run Calibration\n\n',est_dist & ~prob_kc); +end; + + +return; \ No newline at end of file diff --git a/src/g_calib/go_calib_optim_iter_fisheye.m b/src/g_calib/go_calib_optim_iter_fisheye.m new file mode 100755 index 0000000000000000000000000000000000000000..d95de6da79c53b1a214f6def3d498c57ecf8ab64 --- /dev/null +++ b/src/g_calib/go_calib_optim_iter_fisheye.m @@ -0,0 +1,618 @@ +%go_calib_optim_iter_fisheye +% +%Main calibration function. Computes the intrinsic and extrinsic parameters. +%for fisheye cameras. +%Runs as a script. +% +%INPUT: x_1,x_2,x_3,...: Feature locations on the images +% X_1,X_2,X_3,...: Corresponding grid coordinates +% +%OUTPUT: fc: Camera focal length +% cc: Principal point coordinates +% alpha_c: Skew coefficient +% kc: Fisheye distortion coefficients +% KK: The camera matrix (containing fc and cc) +% omc_1,omc_2,omc_3,...: 3D rotation vectors attached to the grid positions in space +% Tc_1,Tc_2,Tc_3,...: 3D translation vectors attached to the grid positions in space +% Rc_1,Rc_2,Rc_3,...: 3D rotation matrices corresponding to the omc vectors +% +%Method: Minimizes the pixel reprojection error in the least squares sense over the intrinsic +% camera parameters, and the extrinsic parameters (3D locations of the grids in space) +% +%Note: If the intrinsic camera parameters (fc, cc, kc) do not exist before, they are initialized through +% the function init_intrinsic_param_fisheye.m. Otherwise, the variables in memory are used as initial guesses. +% +%Note: The row vector active_images consists of zeros and ones. To deactivate an image, set the +% corresponding entry in the active_images vector to zero. +% +%VERY IMPORTANT: This function works for 2D and 3D calibration rigs, except for init_intrinsic_param.m +%that is so far implemented to work only with 2D rigs. +%In the future, a more general function will be there. +%For now, if using a 3D calibration rig, quick_init is set to 1 for an easy initialization of the focal length + +if ~exist('desactivated_images'), + desactivated_images = []; +end; + + +if exist('kc') + if length(kc) > 4, + clear kc; + end; +end; + +if exist('est_dist') + if length(est_dist) > 4, + clear est_dist; + end; +end; + + +if ~exist('est_aspect_ratio'), + est_aspect_ratio = 1; +end; + +if ~exist('est_fc'); + est_fc = [1;1]; % Set to zero if you do not want to estimate the focal length (it may be useful! believe it or not!) +end; + +if ~exist('recompute_extrinsic'), + recompute_extrinsic = 1; % Set this variable to 0 in case you do not want to recompute the extrinsic parameters + % at each iterstion. +end; + +if ~exist('MaxIter'), + MaxIter = 30; % Maximum number of iterations in the gradient descent +end; + +if ~exist('check_cond'), + check_cond = 1; % Set this variable to 0 in case you don't want to extract view dynamically +end; + +if ~exist('center_optim'), + center_optim = 1; %%% Set this variable to 0 if your do not want to estimate the principal point +end; + +%if exist('est_dist'), +% if length(est_dist) == 4, +% est_dist = [est_dist ; 0]; +% end; +%end; + +if ~exist('est_dist'), + est_dist = [1;1;1;1]; +end; + +if ~exist('est_alpha'), + est_alpha = 0; % by default, do not estimate skew +end; + + +% Little fix in case of stupid values in the binary variables: +center_optim = double(~~center_optim); +est_alpha = double(~~est_alpha); +est_dist = double(~~est_dist); +est_fc = double(~~est_fc); +est_aspect_ratio = double(~~est_aspect_ratio); + + + +fprintf(1,'\n'); + +if ~exist('nx')&~exist('ny'), + fprintf(1,'WARNING: No image size (nx,ny) available. Setting nx=640 and ny=480. If these are not the right values, change values manually.\n'); + nx = 640; + ny = 480; +end; + + +check_active_images; + + +quick_init = 0; % Set to 1 for using a quick init (necessary when using 3D rigs) + + +% Check 3D-ness of the calibration rig: +rig3D = 0; +for kk = ind_active, + eval(['X_kk = X_' num2str(kk) ';']); + if is3D(X_kk), + rig3D = 1; + end; +end; + + +if center_optim & (length(ind_active) < 2) & ~rig3D, + fprintf(1,'WARNING: Principal point rejected from the optimization when using one image and planar rig (center_optim = 1).\n'); + center_optim = 0; %%% when using a single image, please, no principal point estimation!!! + est_alpha = 0; +end; + +if ~exist('dont_ask'), + dont_ask = 0; +end; + +if center_optim & (length(ind_active) < 5) & ~rig3D, + fprintf(1,'WARNING: The principal point estimation may be unreliable (using less than 5 images for calibration).\n'); + %if ~dont_ask, + % quest = input('Are you sure you want to keep the principal point in the optimization process? ([]=yes, other=no) '); + % center_optim = isempty(quest); + %end; +end; + + +% A quick fix for solving conflict +if ~isequal(est_fc,[1;1]), + est_aspect_ratio=1; +end; +if ~est_aspect_ratio, + est_fc=[1;1]; +end; + + +if ~est_aspect_ratio, + fprintf(1,'Aspect ratio not optimized (est_aspect_ratio = 0) -> fc(1)=fc(2). Set est_aspect_ratio to 1 for estimating aspect ratio.\n'); +else + if isequal(est_fc,[1;1]), + fprintf(1,'Aspect ratio optimized (est_aspect_ratio = 1) -> both components of fc are estimated (DEFAULT).\n'); + end; +end; + +if ~isequal(est_fc,[1;1]), + if isequal(est_fc,[1;0]), + fprintf(1,'The first component of focal (fc(1)) is estimated, but not the second one (est_fc=[1;0])\n'); + else + if isequal(est_fc,[0;1]), + fprintf(1,'The second component of focal (fc(1)) is estimated, but not the first one (est_fc=[0;1])\n'); + else + fprintf(1,'The focal vector fc is not optimized (est_fc=[0;0])\n'); + end; + end; +end; + + +if ~center_optim, % In the case where the principal point is not estimated, keep it at the center of the image + fprintf(1,'Principal point not optimized (center_optim=0). '); + if ~exist('cc'), + fprintf(1,'It is kept at the center of the image.\n'); + cc = [(nx-1)/2;(ny-1)/2]; + else + fprintf(1,'Note: to set it in the middle of the image, clear variable cc, and run calibration again.\n'); + end; +else + fprintf(1,'Principal point optimized (center_optim=1) - (DEFAULT). To reject principal point, set center_optim=0\n'); +end; + + +if ~center_optim & (est_alpha), + fprintf(1,'WARNING: Since there is no principal point estimation (center_optim=0), no skew estimation (est_alpha = 0)\n'); + est_alpha = 0; +end; + +if ~est_alpha, + fprintf(1,'Skew not optimized (est_alpha=0) - (DEFAULT)\n'); + alpha_c = 0; +else + fprintf(1,'Skew optimized (est_alpha=1). To disable skew estimation, set est_alpha=0.\n'); +end; + + +if ~prod(double(est_dist)), + fprintf(1,'Distortion not fully estimated (defined by the variable est_dist):\n'); +end; + + +% Check 3D-ness of the calibration rig: +rig3D = 0; +for kk = ind_active, + eval(['X_kk = X_' num2str(kk) ';']); + if is3D(X_kk), + rig3D = 1; + end; +end; + +% If the rig is 3D, then no choice: the only valid initialization is manual! +if rig3D, + quick_init = 1; +end; + + + +alpha_smooth = 0.2; % set alpha_smooth = 1; for steepest gradient descent + + +% Conditioning threshold for view rejection +thresh_cond = 1e6; + + + +%%% Initialization of the intrinsic parameters (if necessary) + +if ~exist('cc'), + fprintf(1,'Initialization of the principal point at the center of the image.\n'); + cc = [(nx-1)/2;(ny-1)/2]; + alpha_smooth = 0.4; % slow convergence +end; + + +%if exist('kc'), +% if length(kc) == 4; +% fprintf(1,'Adding a new distortion coefficient to kc -> radial distortion model up to the 6th degree'); +% kc = [kc;0]; +% end; +%end; + + + +if ~exist('alpha_c'), + fprintf(1,'Initialization of the image skew to zero.\n'); + alpha_c = 0; + alpha_smooth = 0.4; % slow convergence +end; + +if ~exist('fc')& quick_init, + fc = (max(nx,ny) / pi) * ones(2,1); + est_fc = [1;1]; + alpha_smooth = 0.4; % slow +end; + + +if ~exist('fc'), + % Initialization of the intrinsic parameters: + fprintf(1,'Initialization of the Fisheye intrinsic parameters\n') + init_intrinsic_param_fisheye; % The right way to go (if quick_init is not active)! + alpha_smooth = 0.4; % slow convergence + est_fc = [1;1]; +end; + +if ~exist('kc'), + fprintf(1,'Initialization of the image distortion to zero.\n'); + kc = zeros(4,1); + alpha_smooth = 0.4; % slow convergence +end; + +if ~est_aspect_ratio, + fc(1) = (fc(1)+fc(2))/2; + fc(2) = fc(1); +end; + +if ~prod(double(est_dist)), + % If no distortion estimated, set to zero the variables that are not estimated + kc = kc .* est_dist; +end; + + +if ~prod(double(est_fc)), + fprintf(1,'Warning: The focal length is not fully estimated (est_fc ~= [1;1])\n'); +end; + + +%%% Initialization of the extrinsic parameters for global minimization: +comp_ext_calib_fisheye; + + +%%% Initialization of the global parameter vector: + +init_param = [fc;cc;alpha_c;kc;zeros(6,1)]; + +for kk = 1:n_ima, + eval(['omckk = omc_' num2str(kk) ';']); + eval(['Tckk = Tc_' num2str(kk) ';']); + init_param = [init_param; omckk ; Tckk]; +end; + + + +%-------------------- Main Optimization: + +fprintf(1,'\nMain calibration optimization procedure - Number of images: %d\n',length(ind_active)); + + +param = init_param; +change = 1; + +iter = 0; + +fprintf(1,'Gradient descent iterations: '); + +param_list = param; + + +while (change > 1e-6)&(iter < MaxIter), + + fprintf(1,'%d...',iter+1); + + % To speed up: pre-allocate the memory for the Jacobian JJ3. + % For that, need to compute the total number of points. + + %% The first step consists of updating the whole vector of knowns (intrinsic + extrinsic of active + %% images) through a one step steepest gradient descent. + + + f = param(1:2); + c = param(3:4); + alpha = param(5); + k = param(6:9); + + + % Compute the size of the Jacobian matrix: + N_points_views_active = N_points_views(ind_active); + + JJ3 = sparse([],[],[],15 + 6*n_ima,15 + 6*n_ima,126*n_ima + 225); + ex3 = zeros(15 + 6*n_ima,1); + + + for kk = ind_active, %1:n_ima, + %if active_images(kk), + + omckk = param(15+6*(kk-1) + 1:15+6*(kk-1) + 3); + + Tckk = param(15+6*(kk-1) + 4:15+6*(kk-1) + 6); + + if isnan(omckk(1)), + fprintf(1,'Intrinsic parameters at frame %d do not exist\n',kk); + return; + end; + + eval(['X_kk = X_' num2str(kk) ';']); + eval(['x_kk = x_' num2str(kk) ';']); + + Np = N_points_views(kk); + + if ~est_aspect_ratio, + [x,dxdom,dxdT,dxdf,dxdc,dxdk,dxdalpha] = project_points_fisheye(X_kk,omckk,Tckk,f(1),c,k,alpha); + dxdf = repmat(dxdf,[1 2]); + else + [x,dxdom,dxdT,dxdf,dxdc,dxdk,dxdalpha] = project_points_fisheye(X_kk,omckk,Tckk,f,c,k,alpha); + end; + + exkk = x_kk - x; + + A = [dxdf dxdc dxdalpha dxdk]'; + B = [dxdom dxdT]'; + + JJ3(1:9,1:9) = JJ3(1:9,1:9) + sparse(A*A'); + JJ3(15+6*(kk-1) + 1:15+6*(kk-1) + 6,15+6*(kk-1) + 1:15+6*(kk-1) + 6) = sparse(B*B'); + + AB = sparse(A*B'); + JJ3(1:9,15+6*(kk-1) + 1:15+6*(kk-1) + 6) = AB; + JJ3(15+6*(kk-1) + 1:15+6*(kk-1) + 6,1:9) = (AB)'; + + ex3(1:9) = ex3(1:9) + A*exkk(:); + ex3(15+6*(kk-1) + 1:15+6*(kk-1) + 6) = B*exkk(:); + + + %JJ3(1:10,1:10) = JJ3(1:10,1:10) + sparse(A*A'); + %JJ3(15+6*(kk-1) + 1:15+6*(kk-1) + 6,15+6*(kk-1) + 1:15+6*(kk-1) + 6) = sparse(B*B'); + + %AB = sparse(A*B'); + %JJ3(1:10,15+6*(kk-1) + 1:15+6*(kk-1) + 6) = AB; + %JJ3(15+6*(kk-1) + 1:15+6*(kk-1) + 6,1:10) = (AB)'; + + %ex3(1:10) = ex3(1:10) + A*exkk(:); + %ex3(15+6*(kk-1) + 1:15+6*(kk-1) + 6) = B*exkk(:); + + + % Check if this view is ill-conditioned: + if check_cond, + JJ_kk = B'; %[dxdom dxdT]; + if (cond(JJ_kk)> thresh_cond), + active_images(kk) = 0; + fprintf(1,'\nWarning: View #%d ill-conditioned. This image is now set inactive. (note: to disactivate this option, set check_cond=0)\n',kk) + desactivated_images = [desactivated_images kk]; + param(15+6*(kk-1) + 1:15+6*(kk-1) + 6) = NaN*ones(6,1); + end; + end; + + %end; + + end; + + + % List of active images (necessary if changed): + check_active_images; + + + % The following vector helps to select the variables to update (for only active images): + selected_variables = [est_fc;center_optim*ones(2,1);est_alpha;est_dist;zeros(6,1);reshape(ones(6,1)*active_images,6*n_ima,1)]; + if ~est_aspect_ratio, + if isequal(est_fc,[1;1]) | isequal(est_fc,[1;0]), + selected_variables(2) = 0; + end; + end; + ind_Jac = find(selected_variables)'; + + JJ3 = JJ3(ind_Jac,ind_Jac); + ex3 = ex3(ind_Jac); + + JJ2_inv = inv(JJ3); % not bad for sparse matrices!! + + + % Smoothing coefficient: + + alpha_smooth2 = 1-(1-alpha_smooth)^(iter+1); %set to 1 to undo any smoothing! + + param_innov = alpha_smooth2*JJ2_inv*ex3; + + + param_up = param(ind_Jac) + param_innov; + param(ind_Jac) = param_up; + + + % New intrinsic parameters: + + fc_current = param(1:2); + cc_current = param(3:4); + + if center_optim & ((param(3)<0)|(param(3)>nx)|(param(4)<0)|(param(4)>ny)), + fprintf(1,'Warning: it appears that the principal point cannot be estimated. Setting center_optim = 0\n'); + center_optim = 0; + cc_current = c; + else + cc_current = param(3:4); + end; + + alpha_current = param(5); + kc_current = param(6:9); + + if ~est_aspect_ratio & isequal(est_fc,[1;1]), + fc_current(2) = fc_current(1); + param(2) = param(1); + end; + + % Change on the intrinsic parameters: + change = norm([fc_current;cc_current] - [f;c])/norm([fc_current;cc_current]); + + + %% Second step: (optional) - It makes convergence faster, and the region of convergence LARGER!!! + %% Recompute the extrinsic parameters only using compute_extrinsic.m (this may be useful sometimes) + %% The complete gradient descent method is useful to precisely update the intrinsic parameters. + + + if recompute_extrinsic, + MaxIter2 = 20; + for kk =ind_active, %1:n_ima, + %if active_images(kk), + omc_current = param(15+6*(kk-1) + 1:15+6*(kk-1) + 3); + Tc_current = param(15+6*(kk-1) + 4:15+6*(kk-1) + 6); + eval(['X_kk = X_' num2str(kk) ';']); + eval(['x_kk = x_' num2str(kk) ';']); + [omc_current,Tc_current] = compute_extrinsic_init_fisheye(x_kk,X_kk,fc_current,cc_current,kc_current,alpha_current); + [omckk,Tckk,Rckk,JJ_kk] = compute_extrinsic_refine_fisheye(omc_current,Tc_current,x_kk,X_kk,fc_current,cc_current,kc_current,alpha_current,MaxIter2,thresh_cond); + if check_cond, + if (cond(JJ_kk)> thresh_cond), + active_images(kk) = 0; + fprintf(1,'\nWarning: View #%d ill-conditioned. This image is now set inactive. (note: to disactivate this option, set check_cond=0)\n',kk); + desactivated_images = [desactivated_images kk]; + omckk = NaN*ones(3,1); + Tckk = NaN*ones(3,1); + end; + end; + param(15+6*(kk-1) + 1:15+6*(kk-1) + 3) = omckk; + param(15+6*(kk-1) + 4:15+6*(kk-1) + 6) = Tckk; + %end; + end; + end; + + param_list = [param_list param]; + iter = iter + 1; + +end; + +fprintf(1,'done\n'); + + + +%%%--------------------------- Computation of the error of estimation: + +fprintf(1,'Estimation of uncertainties...'); + + +check_active_images; + +solution = param; + + +% Extraction of the paramters for computing the right reprojection error: + +fc = solution(1:2); +cc = solution(3:4); +alpha_c = solution(5); +kc = solution(6:9); + +for kk = 1:n_ima, + + if active_images(kk), + + omckk = solution(15+6*(kk-1) + 1:15+6*(kk-1) + 3);%*** + Tckk = solution(15+6*(kk-1) + 4:15+6*(kk-1) + 6);%*** + Rckk = rodrigues(omckk); + + else + + omckk = NaN*ones(3,1); + Tckk = NaN*ones(3,1); + Rckk = NaN*ones(3,3); + + end; + + eval(['omc_' num2str(kk) ' = omckk;']); + eval(['Rc_' num2str(kk) ' = Rckk;']); + eval(['Tc_' num2str(kk) ' = Tckk;']); + +end; + + +% Recompute the error (in the vector ex): +comp_error_calib_fisheye; + +sigma_x = std(ex(:)); + +% Compute the size of the Jacobian matrix: +N_points_views_active = N_points_views(ind_active); + +JJ3 = sparse([],[],[],15 + 6*n_ima,15 + 6*n_ima,126*n_ima + 225); + +for kk = ind_active, + + omckk = param(15+6*(kk-1) + 1:15+6*(kk-1) + 3); + Tckk = param(15+6*(kk-1) + 4:15+6*(kk-1) + 6); + + eval(['X_kk = X_' num2str(kk) ';']); + + Np = N_points_views(kk); + + %[x,dxdom,dxdT,dxdf,dxdc,dxdk,dxdalpha] = project_points2(X_kk,omckk,Tckk,fc,cc,kc,alpha_c); + + if ~est_aspect_ratio, + [x,dxdom,dxdT,dxdf,dxdc,dxdk,dxdalpha] = project_points_fisheye(X_kk,omckk,Tckk,fc(1),cc,kc,alpha_c); + dxdf = repmat(dxdf,[1 2]); + else + [x,dxdom,dxdT,dxdf,dxdc,dxdk,dxdalpha] = project_points_fisheye(X_kk,omckk,Tckk,fc,cc,kc,alpha_c); + end; + + A = [dxdf dxdc dxdalpha dxdk]'; + B = [dxdom dxdT]'; + + JJ3(1:9,1:9) = JJ3(1:9,1:9) + sparse(A*A'); + JJ3(15+6*(kk-1) + 1:15+6*(kk-1) + 6,15+6*(kk-1) + 1:15+6*(kk-1) + 6) = sparse(B*B'); + + AB = sparse(A*B'); + JJ3(1:9,15+6*(kk-1) + 1:15+6*(kk-1) + 6) = AB; + JJ3(15+6*(kk-1) + 1:15+6*(kk-1) + 6,1:9) = (AB)'; + +end; + +JJ3 = JJ3(ind_Jac,ind_Jac); + +JJ2_inv = inv(JJ3); % not bad for sparse matrices!! + +param_error = zeros(6*n_ima+15,1); +param_error(ind_Jac) = 3*sqrt(full(diag(JJ2_inv)))*sigma_x; + +solution_error = param_error; + +if ~est_aspect_ratio & isequal(est_fc,[1;1]), + solution_error(2) = solution_error(1); +end; + + +%%% Extraction of the final intrinsic and extrinsic paramaters: + +extract_parameters_fisheye; + +fprintf(1,'done\n'); + + +fprintf(1,'\n\nCalibration results after optimization (with uncertainties):\n\n'); +fprintf(1,'Focal Length: fc = [ %3.5f %3.5f ] ± [ %3.5f %3.5f ]\n',[fc;fc_error]); +fprintf(1,'Principal point: cc = [ %3.5f %3.5f ] ± [ %3.5f %3.5f ]\n',[cc;cc_error]); +fprintf(1,'Skew: alpha_c = [ %3.5f ] ± [ %3.5f ] => angle of pixel axes = %3.5f ± %3.5f degrees\n',[alpha_c;alpha_c_error],90 - atan(alpha_c)*180/pi,atan(alpha_c_error)*180/pi); +fprintf(1,'Fisheye Distortion: kc = [ %3.5f %3.5f %3.5f %3.5f ] ± [ %3.5f %3.5f %3.5f %3.5f ]\n',[kc;kc_error]); +fprintf(1,'Pixel error: err = [ %3.5f %3.5f ]\n\n',err_std); +fprintf(1,'Note: The numerical errors are approximately three times the standard deviations (for reference).\n\n\n') +%fprintf(1,' For accurate (and stable) error estimates, it is recommended to run Calibration once again.\n\n\n') + + + +return; \ No newline at end of file diff --git a/src/g_calib/go_calib_optim_iter_weak.m b/src/g_calib/go_calib_optim_iter_weak.m new file mode 100755 index 0000000000000000000000000000000000000000..1385d0c9ed181cae9a6008b3420caf69f7456552 --- /dev/null +++ b/src/g_calib/go_calib_optim_iter_weak.m @@ -0,0 +1,673 @@ +%%% WARNING!!! This code is not in working condition yet. +%%% AS A RESULT, IT IS NOT SUPPORTED!!! +%%% -Jean-Yves Bouguet + +%go_calib_optim_iter_weak +% +%Main calibration function. Computes the intrinsic andextrinsic parameters +%for the weak perspective camera model +%Runs as a script. +% +%INPUT: x_1,x_2,x_3,...: Feature locations on the images +% X_1,X_2,X_3,...: Corresponding grid coordinates +% +%OUTPUT: fc: Camera focal length +% cc: Principal point coordinates +% alpha_c: Skew coefficient +% kc: Distortion coefficients +% KK: The camera matrix (containing fc and cc) +% omc_1,omc_2,omc_3,...: 3D rotation vectors attached to the grid positions in space +% Tc_1,Tc_2,Tc_3,...: 3D translation vectors attached to the grid positions in space +% Rc_1,Rc_2,Rc_3,...: 3D rotation matrices corresponding to the omc vectors +% +%Method: Minimizes the pixel reprojection error in the least squares sense over the intrinsic +% camera parameters, and the extrinsic parameters (3D locations of the grids in space) +% +%Note: If the intrinsic camera parameters (fc, cc, kc) do not exist before, they are initialized through +% the function init_intrinsic_param.m. Otherwise, the variables in memory are used as initial guesses. +% +%Note: The row vector active_images consists of zeros and ones. To deactivate an image, set the +% corresponding entry in the active_images vector to zero. +% +%VERY IMPORTANT: This function works for 2D and 3D calibration rigs, except for init_intrinsic_param.m +%that is so far implemented to work only with 2D rigs. +%In the future, a more general function will be there. +%For now, if using a 3D calibration rig, quick_init is set to 1 for an easy initialization of the focal length + + + +%%% Little modifications for weak perspective model: +center_optim = 0; % No center estimation for weak perspective model +%est_dist = zeros(5,1); % No distortion for weak perspective model +%est_alpha = 0; % No skew for weak perspective model + + + +if ~exist('desactivated_images'), + desactivated_images = []; +end; + +if ~exist('est_aspect_ratio'), + est_aspect_ratio = 1; +end; + +if ~exist('est_fc'); + est_fc = [1;1]; % Set to zero if you do not want to estimate the focal length (it may be useful! believe it or not!) +end; + +if ~exist('recompute_extrinsic'), + recompute_extrinsic = 1; % Set this variable to 0 in case you do not want to recompute the extrinsic parameters + % at each iterstion. +end; + + +if ~exist('MaxIter'), + MaxIter = 30; % Maximum number of iterations in the gradient descent +end; + +if ~exist('check_cond'), + check_cond = 1; % Set this variable to 0 in case you don't want to extract view dynamically +end; + +if ~exist('center_optim'), + center_optim = 1; %%% Set this variable to 0 if your do not want to estimate the principal point +end; + +if exist('est_dist'), + if length(est_dist) == 4, + est_dist = [est_dist ; 0]; + end; +end; + +if ~exist('est_dist'), + est_dist = [1;1;1;1;0]; +end; + +if ~exist('est_alpha'), + est_alpha = 0; % by default, do not estimate skew +end; + + +% Little fix in case of stupid values in the binary variables: +center_optim = double(~~center_optim); +est_alpha = double(~~est_alpha); +est_dist = double(~~est_dist); +est_fc = double(~~est_fc); +est_aspect_ratio = double(~~est_aspect_ratio); + + + +fprintf(1,'\n'); + +if ~exist('nx')&~exist('ny'), + fprintf(1,'WARNING: No image size (nx,ny) available. Setting nx=640 and ny=480. If these are not the right values, change values manually.\n'); + nx = 640; + ny = 480; +end; + + +check_active_images; + + +quick_init = 0; % Set to 1 for using a quick init (necessary when using 3D rigs) + + +% Check 3D-ness of the calibration rig: +rig3D = 0; +for kk = ind_active, + eval(['X_kk = X_' num2str(kk) ';']); + if is3D(X_kk), + rig3D = 1; + end; +end; + + +if center_optim & (length(ind_active) < 2) & ~rig3D, + fprintf(1,'WARNING: Principal point rejected from the optimization when using one image and planar rig (center_optim = 1).\n'); + center_optim = 0; %%% when using a single image, please, no principal point estimation!!! + est_alpha = 0; +end; + +if ~exist('dont_ask'), + dont_ask = 0; +end; + +if center_optim & (length(ind_active) < 5) & ~rig3D, + fprintf(1,'WARNING: The principal point estimation may be unreliable (using less than 5 images for calibration).\n'); + %if ~dont_ask, + % quest = input('Are you sure you want to keep the principal point in the optimization process? ([]=yes, other=no) '); + % center_optim = isempty(quest); + %end; +end; + + +% A quick fix for solving conflict +if ~isequal(est_fc,[1;1]), + est_aspect_ratio=1; +end; +if ~est_aspect_ratio, + est_fc=[1;1]; +end; + + +if ~est_aspect_ratio, + fprintf(1,'Aspect ratio not optimized (est_aspect_ratio = 0) -> fc(1)=fc(2). Set est_aspect_ratio to 1 for estimating aspect ratio.\n'); +else + if isequal(est_fc,[1;1]), + fprintf(1,'Aspect ratio optimized (est_aspect_ratio = 1) -> both components of fc are estimated (DEFAULT).\n'); + end; +end; + +if ~isequal(est_fc,[1;1]), + if isequal(est_fc,[1;0]), + fprintf(1,'The first component of focal (fc(1)) is estimated, but not the second one (est_fc=[1;0])\n'); + else + if isequal(est_fc,[0;1]), + fprintf(1,'The second component of focal (fc(1)) is estimated, but not the first one (est_fc=[0;1])\n'); + else + fprintf(1,'The focal vector fc is not optimized (est_fc=[0;0])\n'); + end; + end; +end; + + +if ~center_optim, % In the case where the principal point is not estimated, keep it at the center of the image + fprintf(1,'Principal point not optimized (center_optim=0). Default state for weak perspective model'); + if ~exist('cc'), + fprintf(1,'It is kept at the center of the image.\n'); + cc = [(nx-1)/2;(ny-1)/2]; + else + fprintf(1,'Note: to set it in the middle of the image, clear variable cc, and run calibration again.\n'); + end; +else + fprintf(1,'Principal point optimized (center_optim=1) - (DEFAULT). To reject principal point, set center_optim=0\n'); +end; + + +if ~center_optim & (est_alpha), + fprintf(1,'WARNING: Since there is no principal point estimation (center_optim=0), no skew estimation (est_alpha = 0)\n'); + est_alpha = 0; +end; + +if ~est_alpha, + fprintf(1,'Skew not optimized (est_alpha=0) - (DEFAULT)\n'); + alpha_c = 0; +else + fprintf(1,'Skew optimized (est_alpha=1). To disable skew estimation, set est_alpha=0.\n'); +end; + + +if ~prod(double(est_dist)), + fprintf(1,'Distortion not fully estimated (defined by the variable est_dist):\n'); + if ~est_dist(1), + fprintf(1,' Second order distortion not estimated (est_dist(1)=0) - (DEFAULT for weak perspective model) . \n'); + end; + if ~est_dist(2), + fprintf(1,' Fourth order distortion not estimated (est_dist(2)=0) - (DEFAULT for weak perspective model) .\n'); + end; + if ~est_dist(5), + fprintf(1,' Sixth order distortion not estimated (est_dist(5)=0) - (DEFAULT for weak perspective model) .\n'); + end; + if ~prod(double(est_dist(3:4))), + fprintf(1,' Tangential distortion not estimated (est_dist(3:4)~=[1;1]) - (DEFAULT for weak perspective model) .\n'); + end; +end; + + +% Check 3D-ness of the calibration rig: +rig3D = 0; +for kk = ind_active, + eval(['X_kk = X_' num2str(kk) ';']); + if is3D(X_kk), + rig3D = 1; + end; +end; + +% If the rig is 3D, then no choice: the only valid initialization is manual! +if rig3D, + quick_init = 1; +end; + + + +alpha_smooth = 1; % set alpha_smooth = 1; for steepest gradient descent + + +% Conditioning threshold for view rejection +thresh_cond = 1e6; + + + +%% Initialization of the intrinsic parameters (if necessary) + +if ~exist('cc'), + fprintf(1,'Initialization of the principal point at the center of the image.\n'); + cc = [(nx-1)/2;(ny-1)/2]; + alpha_smooth = 0.4; % slow convergence +end; + + +if exist('kc'), + if length(kc) == 4; + fprintf(1,'Adding a new distortion coefficient to kc -> radial distortion model up to the 6th degree'); + kc = [kc;0]; + end; +end; + + +if ~exist('kc'), + fprintf(1,'Initialization of the image distortion to zero.\n'); + kc = zeros(5,1); + alpha_smooth = 0.4; % slow convergence +end; + +if ~exist('alpha_c'), + fprintf(1,'Initialization of the image skew to zero.\n'); + alpha_c = 0; + alpha_smooth = 0.4; % slow convergence +end; + +if ~exist('fc')& quick_init, + FOV_angle = 35; % Initial camera field of view in degrees + fprintf(1,['Initialization of the focal length to a FOV of ' num2str(FOV_angle) ' degrees.\n']); + fc = (nx/2)/tan(pi*FOV_angle/360) * ones(2,1); + est_fc = [1;1]; + alpha_smooth = 0.4; % slow +end; + + +if ~exist('fc'), + % Initialization of the intrinsic parameters: + fprintf(1,'Initialization of the intrinsic parameters using the vanishing points of planar patterns.\n') + init_intrinsic_param; % The right way to go (if quick_init is not active)! + alpha_smooth = 0.4; % slow convergence + est_fc = [1;1]; +end; + + +if ~est_aspect_ratio, + fc(1) = (fc(1)+fc(2))/2; + fc(2) = fc(1); +end; + +if ~prod(double(est_dist)), + % If no distortion estimated, set to zero the variables that are not estimated + kc = kc .* est_dist; +end; + + +if ~prod(double(est_fc)), + fprintf(1,'Warning: The focal length is not fully estimated (est_fc ~= [1;1])\n'); +end; + + +%%% Initialization of the extrinsic parameters for global minimization: +comp_ext_calib; + + + +%%% Initialization of the global parameter vector: + +init_param = [fc;cc;alpha_c;kc;zeros(5,1)]; + +for kk = 1:n_ima, + eval(['omckk = omc_' num2str(kk) ';']); + eval(['Tckk = Tc_' num2str(kk) ';']); + init_param = [init_param; omckk ; Tckk]; +end; + + + +%-------------------- Main Optimization: + +fprintf(1,'\nMain calibration optimization procedure - Number of images: %d\n',length(ind_active)); + + +param = init_param; +change = 1; + +iter = 0; + +fprintf(1,'Gradient descent iterations: '); + +param_list = param; + + +% Compute the average distance of all the points to the camera: +Zave_list = zeros(1,length(ind_active)); +for ii = 1:length(ind_active), + kk = ind_active(ii); + eval(['X_kk = X_' num2str(kk) ';']); + omckk = param(15+6*(kk-1) + 1:15+6*(kk-1) + 3); + Tckk = param(15+6*(kk-1) + 4:15+6*(kk-1) + 6); + Y_kk = rigid_motion(X_kk,omckk,Tckk); + Zave_list(ii) = mean(Y_kk(3,:)); +end; + +Zave = sum(N_points_views .* Zave_list) / sum(N_points_views); + +fprintf(1,'Weak Perspective Camera Calibration - Setting the average depth of the scene at Zave = %.6f\n',Zave); + + +while (change > 1e-9)&(iter < MaxIter), + + fprintf(1,'%d...',iter+1); + + f = param(1:2); + c = param(3:4); + alpha = param(5); + k = param(6:10); + + %%% Make sure that all the Z translations satisfy the average depth constraint: + % Compute the average distance of all the points to the camera: + Zave_list = zeros(1,length(ind_active)); + for ii = 1:length(ind_active), + kk = ind_active(ii); + eval(['X_kk = X_' num2str(kk) ';']); + omckk = param(15+6*(kk-1) + 1:15+6*(kk-1) + 3); + Tckk = param(15+6*(kk-1) + 4:15+6*(kk-1) + 6); + Y_kk = rigid_motion(X_kk,omckk,Tckk); + Zave_list(ii) = mean(Y_kk(3,:)); + end; + + Zave2 = sum(N_points_views .* Zave_list) / sum(N_points_views); + + change_T = Zave2 - Zave; + + for ii = 1:length(ind_active), + kk = ind_active(ii); + Tckk = param(15+6*(kk-1) + 4:15+6*(kk-1) + 6); + %Tckk2 = Tckk .* [1;1;ratio_T]; + Tckk2 = Tckk - [0;0;change_T]; + param(15+6*(kk-1) + 4:15+6*(kk-1) + 6) = Tckk2; + end; + + + % To speed up: pre-allocate the memory for the Jacobian JJ3. + % For that, need to compute the total number of points. + + %% The first step consists of updating the whole vector of knowns (intrinsic + extrinsic of active + %% images) through a one step steepest gradient descent. + + + + + + % Compute the size of the Jacobian matrix: + N_points_views_active = N_points_views(ind_active); + + JJ3 = sparse([],[],[],15 + 6*n_ima,15 + 6*n_ima,126*n_ima + 225); + ex3 = zeros(15 + 6*n_ima,1); + + + for kk = ind_active, %1:n_ima, + + omckk = param(15+6*(kk-1) + 1:15+6*(kk-1) + 3); + Tckk = param(15+6*(kk-1) + 4:15+6*(kk-1) + 6); + + if isnan(omckk(1)), + fprintf(1,'Intrinsic parameters at frame %d do not exist\n',kk); + return; + end; + + eval(['X_kk = X_' num2str(kk) ';']); + eval(['x_kk = x_' num2str(kk) ';']); + + Np = N_points_views(kk); + + if ~est_aspect_ratio, + [x,dxdom,dxdT,dxdf,dxdc,dxdk,dxdalpha] = project_points_weak(X_kk,omckk,Tckk,f(1),c,k,alpha,Zave); + dxdf = repmat(dxdf,[1 2]); + else + [x,dxdom,dxdT,dxdf,dxdc,dxdk,dxdalpha] = project_points_weak(X_kk,omckk,Tckk,f,c,k,alpha,Zave); + end; + + exkk = x_kk - x; + + A = [dxdf dxdc dxdalpha dxdk]'; + B = [dxdom dxdT]'; + + JJ3(1:10,1:10) = JJ3(1:10,1:10) + sparse(A*A'); + JJ3(15+6*(kk-1) + 1:15+6*(kk-1) + 6,15+6*(kk-1) + 1:15+6*(kk-1) + 6) = sparse(B*B'); + + AB = sparse(A*B'); + JJ3(1:10,15+6*(kk-1) + 1:15+6*(kk-1) + 6) = AB; + JJ3(15+6*(kk-1) + 1:15+6*(kk-1) + 6,1:10) = (AB)'; + + ex3(1:10) = ex3(1:10) + A*exkk(:); + ex3(15+6*(kk-1) + 1:15+6*(kk-1) + 6) = B*exkk(:); + + % Check if this view is ill-conditioned: + if 0, %check_cond, + JJ_kk = B'; %[dxdom dxdT]; + if (cond(JJ_kk)> thresh_cond), + active_images(kk) = 0; + fprintf(1,'\nWarning: View #%d ill-conditioned. This image is now set inactive. (note: to disactivate this option, set check_cond=0)\n',kk) + desactivated_images = [desactivated_images kk]; + param(15+6*(kk-1) + 1:15+6*(kk-1) + 6) = NaN*ones(6,1); + end; + end; + + end; + + + % List of active images (necessary if changed): + check_active_images; + + + % The following vector helps to select the variables to update (for only active images): + selected_variables = [est_fc;center_optim*ones(2,1);est_alpha;est_dist;zeros(5,1);reshape(ones(6,1)*active_images,6*n_ima,1)]; + if ~est_aspect_ratio, + if isequal(est_fc,[1;1]) | isequal(est_fc,[1;0]), + selected_variables(2) = 0; + end; + end; + ind_Jac = find(selected_variables)'; + + JJ4 = JJ3(ind_Jac,ind_Jac); + ex4 = ex3(ind_Jac); + + + % Try to make the inversion work: + [U,S,V] = svd(full(JJ4)); + + + + if 0, + s = diag(S); + figure(100); + semilogy(s); + end; + + n_reject = length(ind_active); + + U = U(:,1:end-n_reject); + S = S(1:end-n_reject,1:end-n_reject); + + + JJ2_inv = U * inv(S) * U'; + + %JJ2_inv = inv(JJ4); % not bad for sparse matrices!! + + + % Smoothing coefficient: + + alpha_smooth2 = 1-(1-alpha_smooth)^(iter+1); %set to 1 to undo any smoothing! + + param_innov = alpha_smooth2*JJ2_inv*ex4; + + + param_up = param(ind_Jac) + param_innov; + param(ind_Jac) = param_up; + + + % New intrinsic parameters: + + fc_current = param(1:2); + cc_current = param(3:4); + alpha_current = param(5); + kc_current = param(6:10); + + + if ~est_aspect_ratio & isequal(est_fc,[1;1]), + fc_current(2) = fc_current(1); + param(2) = param(1); + end; + + % Change on the intrinsic parameters: + change = norm([fc_current;cc_current] - [f;c])/norm([fc_current;cc_current]); + + param_list = [param_list param]; + iter = iter + 1; + +end; + +fprintf(1,'done\n'); + + + +%%%--------------------------- Computation of the error of estimation: + +fprintf(1,'Estimation of uncertainties...'); + + +check_active_images; + +solution = param; + + +% Extraction of the paramters for computing the right reprojection error: + +fc = solution(1:2); +cc = solution(3:4); +alpha_c = solution(5); +kc = solution(6:10); + +for kk = 1:n_ima, + + if active_images(kk), + + omckk = solution(15+6*(kk-1) + 1:15+6*(kk-1) + 3);%*** + Tckk = solution(15+6*(kk-1) + 4:15+6*(kk-1) + 6);%*** + Rckk = rodrigues(omckk); + + else + + omckk = NaN*ones(3,1); + Tckk = NaN*ones(3,1); + Rckk = NaN*ones(3,3); + + end; + + eval(['omc_' num2str(kk) ' = omckk;']); + eval(['Rc_' num2str(kk) ' = Rckk;']); + eval(['Tc_' num2str(kk) ' = Tckk;']); + +end; + + +% Recompute the error (in the vector ex): +comp_error_calib; + +sigma_x = std(ex(:)); + +% Compute the size of the Jacobian matrix: +N_points_views_active = N_points_views(ind_active); + +JJ3 = sparse([],[],[],15 + 6*n_ima,15 + 6*n_ima,126*n_ima + 225); + +for kk = ind_active, + + omckk = param(15+6*(kk-1) + 1:15+6*(kk-1) + 3); + Tckk = param(15+6*(kk-1) + 4:15+6*(kk-1) + 6); + + eval(['X_kk = X_' num2str(kk) ';']); + + Np = N_points_views(kk); + + %[x,dxdom,dxdT,dxdf,dxdc,dxdk,dxdalpha] = project_points_weak(X_kk,omckk,Tckk,fc,cc,kc,alpha_c,Zave); + + if ~est_aspect_ratio, + [x,dxdom,dxdT,dxdf,dxdc,dxdk,dxdalpha] = project_points_weak(X_kk,omckk,Tckk,fc(1),cc,kc,alpha_c,Zave); + dxdf = repmat(dxdf,[1 2]); + else + [x,dxdom,dxdT,dxdf,dxdc,dxdk,dxdalpha] = project_points_weak(X_kk,omckk,Tckk,fc,cc,kc,alpha_c,Zave); + end; + + A = [dxdf dxdc dxdalpha dxdk]'; + B = [dxdom dxdT]'; + + JJ3(1:10,1:10) = JJ3(1:10,1:10) + sparse(A*A'); + JJ3(15+6*(kk-1) + 1:15+6*(kk-1) + 6,15+6*(kk-1) + 1:15+6*(kk-1) + 6) = sparse(B*B'); + + AB = sparse(A*B'); + JJ3(1:10,15+6*(kk-1) + 1:15+6*(kk-1) + 6) = AB; + JJ3(15+6*(kk-1) + 1:15+6*(kk-1) + 6,1:10) = (AB)'; + +end; + +JJ3 = JJ3(ind_Jac,ind_Jac); +% Try to make the inversion work: +[U,S,V] = svd(full(JJ3)); +n_reject = length(ind_active); +U = U(:,1:end-n_reject); +S = S(1:end-n_reject,1:end-n_reject); + +JJ2_inv = U * inv(S) * U'; +%JJ2_inv = inv(JJ3); % not bad for sparse matrices!! + +param_error = zeros(6*n_ima+15,1); +param_error(ind_Jac) = 3*sqrt(full(diag(JJ2_inv)))*sigma_x; + +solution_error = param_error; + +if ~est_aspect_ratio & isequal(est_fc,[1;1]), + solution_error(2) = solution_error(1); +end; + + +%%% Extraction of the final intrinsic and extrinsic paramaters: + +extract_parameters; + +fprintf(1,'done\n'); + + +fprintf(1,'\n\nCalibration results after optimization for weak perspective camera model (with uncertainties):\n\n'); +fprintf(1,'Focal Length: fc = [ %3.5f %3.5f ] ± [ %3.5f %3.5f ]\n',[fc;fc_error]); +fprintf(1,'Principal point: cc = [ %3.5f %3.5f ] ± [ %3.5f %3.5f ]\n',[cc;cc_error]); +fprintf(1,'Skew: alpha_c = [ %3.5f ] ± [ %3.5f ] => angle of pixel axes = %3.5f ± %3.5f degrees\n',[alpha_c;alpha_c_error],90 - atan(alpha_c)*180/pi,atan(alpha_c_error)*180/pi); +fprintf(1,'Distortion: kc = [ %3.5f %3.5f %3.5f %3.5f %5.5f ] ± [ %3.5f %3.5f %3.5f %3.5f %5.5f ]\n',[kc;kc_error]); +fprintf(1,'Pixel error: err = [ %3.5f %3.5f ]\n\n',err_std); +fprintf(1,'Note: The numerical errors are approximately three times the standard deviations (for reference).\n\n\n') +%fprintf(1,' For accurate (and stable) error estimates, it is recommended to run Calibration once again.\n\n\n') + + + +%%% Some recommendations to the user to reject some of the difficult unkowns... Still in debug mode. + +alpha_c_min = alpha_c - alpha_c_error/2; +alpha_c_max = alpha_c + alpha_c_error/2; + +if (alpha_c_min < 0) & (alpha_c_max > 0), + fprintf(1,'Recommendation: The skew coefficient alpha_c is found to be equal to zero (within its uncertainty).\n'); + fprintf(1,' You may want to reject it from the optimization by setting est_alpha=0 and run Calibration\n\n'); +end; + +kc_min = kc - kc_error/2; +kc_max = kc + kc_error/2; + +prob_kc = (kc_min < 0) & (kc_max > 0); + +if ~(prob_kc(3) & prob_kc(4)) + prob_kc(3:4) = [0;0]; +end; + + +if sum(prob_kc), + fprintf(1,'Recommendation: Some distortion coefficients are found equal to zero (within their uncertainties).\n'); + fprintf(1,' To reject them from the optimization set est_dist=[%d;%d;%d;%d;%d] and run Calibration\n\n',est_dist & ~prob_kc); +end; + + +return; \ No newline at end of file diff --git a/src/g_calib/go_calib_optim_no_read.m b/src/g_calib/go_calib_optim_no_read.m new file mode 100755 index 0000000000000000000000000000000000000000..057f6b08b75c2c2e79b8a81740473955b4fa4028 --- /dev/null +++ b/src/g_calib/go_calib_optim_no_read.m @@ -0,0 +1,69 @@ +%go_calib_optim +% +%Main calibration function. Computes the intrinsic andextrinsic parameters. +%Runs as a script. +% +%INPUT: x_1,x_2,x_3,...: Feature locations on the images +% X_1,X_2,X_3,...: Corresponding grid coordinates +% +%OUTPUT: fc: Camera focal length +% cc: Principal point coordinates +% alpha_c: Skew coefficient +% kc: Distortion coefficients +% KK: The camera matrix (containing fc and cc) +% omc_1,omc_2,omc_3,...: 3D rotation vectors attached to the grid positions in space +% Tc_1,Tc_2,Tc_3,...: 3D translation vectors attached to the grid positions in space +% Rc_1,Rc_2,Rc_3,...: 3D rotation matrices corresponding to the omc vectors +% +%Method: Minimizes the pixel reprojection error in the least squares sense over the intrinsic +% camera parameters, and the extrinsic parameters (3D locations of the grids in space) +% +%Note: If the intrinsic camera parameters (fc, cc, kc) do not exist before, they are initialized through +% the function init_intrinsic_param.m. Otherwise, the variables in memory are used as initial guesses. +% +%Note: The row vector active_images consists of zeros and ones. To deactivate an image, set the +% corresponding entry in the active_images vector to zero. +% +%VERY IMPORTANT: This function works for 2D and 3D calibration rigs, except for init_intrinsic_param.m +%that is so far implemented to work only with 2D rigs. +%In the future, a more general function will be there. +%For now, if using a 3D calibration rig, set quick_init to 1 for an easy initialization of the focal length + + +if ~exist('n_ima'), + data_calib_no_read; % Load the images + click_calib_no_read; % Extract the corners +end; + + +check_active_images; +check_extracted_images; +check_active_images; +desactivated_images = []; + +recompute_extrinsic = (length(ind_active) < 100); % if there are too many images, do not spend time recomputing the extrinsic parameters twice.. + +if ~exist('rosette_calibration', 'var') + rosette_calibration = 0; +end; + +if (rosette_calibration) + %%% Special Setting for the Rosette: + est_dist = ones(5,1); +end; + +%%% MAIN OPTIMIZATION CALL!!!!! (look into this function for the details of implementation) +go_calib_optim_iter; + +if ~isempty(desactivated_images), + param_list_save = param_list; + fprintf(1,'\nNew optimization including the images that have been deactivated during the previous optimization.\n'); + active_images(desactivated_images) = ones(1,length(desactivated_images)); + desactivated_images = []; + go_calib_optim_iter; + if ~isempty(desactivated_images), + fprintf(1,['List of images left desactivated: ' num2str(desactivated_images) '\n' ] ); + end; + param_list = [param_list_save(:,1:end-1) param_list]; +end; + diff --git a/src/g_calib/go_calib_stereo.m b/src/g_calib/go_calib_stereo.m new file mode 100755 index 0000000000000000000000000000000000000000..2c3a832ea1e2b2edd28a6e8d3eecbaa94925b545 --- /dev/null +++ b/src/g_calib/go_calib_stereo.m @@ -0,0 +1,385 @@ +% go_calib_stereo.m +% +% Script for Calibrating a stereo rig (two cameras, internal and external calibration): +% +% It is assumed that the two cameras (left and right) have been calibrated with the pattern at the same 3D locations, and the same points +% on the pattern (select the same grid points). Therefore, in particular, the same number of images were used to calibrate both cameras. +% +% +% Main output variables: +% om, R, T: relative rotation and translation of the right camera wrt the left camera +% fc_left, cc_left, kc_left, alpha_c_left, KK_left: New intrinsic parameters of the left camera +% fc_right, cc_right, kc_right, alpha_c_right, KK_right: New intrinsic parameters of the right camera +% +% Both sets of intrinsic parameters are equivalent to the classical {fc,cc,kc,alpha_c,KK} described online at: +% http://www.vision.caltech.edu/bouguetj/calib_doc/parameters.html +% +% Note: If you do not want to recompute the intinsic parameters, through stereo calibration you may want to set +% recompute_intrinsic_right and recompute_intrinsic_left to zero. Default: 1 +% +% Definition of the extrinsic parameters: R and om are related through the rodrigues formula (R=rodrigues(om)). +% Consider a point P of coordinates XL and XR in the left and right camera reference frames respectively. +% XL and XR are related to each other through the following rigid motion transformation: +% XR = R * XL + T +% R and T (or equivalently om and T) fully describe the relative displacement of the two cameras. +% +% +% If the Warning message "Disabling view kk - Reason: the left and right images are found inconsistent" is encountered, that probably +% means that for the kkth pair of images, the left and right images are found to have captured the calibration pattern at two +% different locations in space. That means that the two views are not consistent, and therefore cannot be used for stereo calibration. +% When capturing your images, make sure that you do not move the calibration pattern between capturing the left and the right images. +% The pattwern can (and should) be moved in space only between two sets of (left,right) images. +% Another reason for inconsistency is that you selected a different set of points on the pattern when running the separate calibrations +% (leading to the two files Calib_Results_left.mat and Calib_Results_left.mat). Make sure that the same points are selected in the +% two separate calibration. In other words, the points need to correspond. +% For disabling this process of inconsistent image pairs detection, set the variable 'inconsistent_pairs_detection' to zero + + + +if ~exist('inconsistent_pairs_detection'), + inconsistent_pairs_detection = 1; +end; + + + +if inconsistent_pairs_detection, + %- This threshold is used only to automatically identify non-consistant image pairs (set to Infinity to not reject pairs) + threshold = 50; %1.673; %1e10; %50; +else + threshold = Inf; +end; + + +if ~exist('recompute_intrinsic_right'), + recompute_intrinsic_right = 1; +end; + + +if ~exist('recompute_intrinsic_left'), + recompute_intrinsic_left = 1; +end; + +center_optim_left_st = center_optim_left; +est_alpha_left_st = est_alpha_left; +est_dist_left_st = est_dist_left; +est_fc_left_st = est_fc_left; +est_aspect_ratio_left_st = est_aspect_ratio_left; % just to fix conflicts +center_optim_right_st = center_optim_right; +est_alpha_right_st = est_alpha_right; +est_dist_right_st = est_dist_right; +est_fc_right_st = est_fc_right; +est_aspect_ratio_right_st = est_aspect_ratio_right; % just to fix conflicts + +if ~recompute_intrinsic_left, + fprintf(1,'\nNo recomputation of the intrinsic parameters of the left camera (recompute_intrinsic_left = 0)\n'); + center_optim_left_st = 0; + est_alpha_left_st = 0; + est_dist_left_st = zeros(5,1); + est_fc_left_st = [0;0]; + est_aspect_ratio_left_st = 1; % just to fix conflicts +else + fprintf(1,'\nRecomputation of the intrinsic parameters of the left camera (recompute_intrinsic_left = 1)\n'); +end; + + +if ~recompute_intrinsic_right, + fprintf(1,'\nNo recomputation of the intrinsic parameters of the right camera (recompute_intrinsic_left = 0)\n'); + center_optim_right_st = 0; + est_alpha_right_st = 0; + est_dist_right_st = zeros(5,1); + est_fc_right_st = [0;0]; + est_aspect_ratio_right_st = 1; % just to fix conflicts +else + fprintf(1,'\nRecomputation of the intrinsic parameters of the right camera (recompute_intrinsic_right = 1)\n'); +end; + +%- Set to zero the entries of the distortion vectors that are not attempted to be estimated. +kc_right = kc_right .* ~~est_dist_right; +kc_left = kc_left .* ~~est_dist_left; + + +active_images = active_images_left & active_images_right; + +history = []; + +fprintf(1,'\nMain stereo calibration optimization procedure - Number of pairs of images: %d\n',length(find(active_images))); +fprintf(1,'Gradient descent iterations: '); + + +MaxIter = 100; +change = 1; +iter = 1; + +while (change > 5e-6)&(iter <= MaxIter), + + + fprintf(1,'%d...',iter); + + % Jacobian: + + J = []; + e = []; + if iter == 1, + e_ref = []; + end; + + param = [fc_left;cc_left;alpha_c_left;kc_left;fc_right;cc_right;alpha_c_right;kc_right;om;T]; + + + for kk = 1:n_ima, + + if active_images(kk), + + % Project the structure onto the left view: + + eval(['Xckk = X_left_' num2str(kk) ';']); + eval(['omckk = omc_left_' num2str(kk) ';']); + eval(['Tckk = Tc_left_' num2str(kk) ';']); + + eval(['xlkk = x_left_' num2str(kk) ';']); + eval(['xrkk = x_right_' num2str(kk) ';']); + + param = [param;omckk;Tckk]; + + % number of points: + Nckk = size(Xckk,2); + + + Jkk = sparse(4*Nckk,20+(1+n_ima)*6); + ekk = zeros(4*Nckk,1); + + + if ~est_aspect_ratio_left, + [xl,dxldomckk,dxldTckk,dxldfl,dxldcl,dxldkl,dxldalphal] = project_points2(Xckk,omckk,Tckk,fc_left(1),cc_left,kc_left,alpha_c_left); + dxldfl = repmat(dxldfl,[1 2]); + else + [xl,dxldomckk,dxldTckk,dxldfl,dxldcl,dxldkl,dxldalphal] = project_points2(Xckk,omckk,Tckk,fc_left,cc_left,kc_left,alpha_c_left); + end; + + + ekk(1:2*Nckk) = xlkk(:) - xl(:); + + Jkk(1:2*Nckk,6*(kk-1)+7+20:6*(kk-1)+7+2+20) = sparse(dxldomckk); + Jkk(1:2*Nckk,6*(kk-1)+7+3+20:6*(kk-1)+7+5+20) = sparse(dxldTckk); + + Jkk(1:2*Nckk,1:2) = sparse(dxldfl); + Jkk(1:2*Nckk,3:4) = sparse(dxldcl); + Jkk(1:2*Nckk,5) = sparse(dxldalphal); + Jkk(1:2*Nckk,6:10) = sparse(dxldkl); + + + % Project the structure onto the right view: + + [omr,Tr,domrdomckk,domrdTckk,domrdom,domrdT,dTrdomckk,dTrdTckk,dTrdom,dTrdT] = compose_motion(omckk,Tckk,om,T); + + if ~est_aspect_ratio_right, + [xr,dxrdomr,dxrdTr,dxrdfr,dxrdcr,dxrdkr,dxrdalphar] = project_points2(Xckk,omr,Tr,fc_right(1),cc_right,kc_right,alpha_c_right); + dxrdfr = repmat(dxrdfr,[1 2]); + else + [xr,dxrdomr,dxrdTr,dxrdfr,dxrdcr,dxrdkr,dxrdalphar] = project_points2(Xckk,omr,Tr,fc_right,cc_right,kc_right,alpha_c_right); + end; + + + ekk(2*Nckk+1:end) = xrkk(:) - xr(:); + + + dxrdom = dxrdomr * domrdom + dxrdTr * dTrdom; + dxrdT = dxrdomr * domrdT + dxrdTr * dTrdT; + + dxrdomckk = dxrdomr * domrdomckk + dxrdTr * dTrdomckk; + dxrdTckk = dxrdomr * domrdTckk + dxrdTr * dTrdTckk; + + + Jkk(2*Nckk+1:end,1+20:3+20) = sparse(dxrdom); + Jkk(2*Nckk+1:end,4+20:6+20) = sparse(dxrdT); + + + Jkk(2*Nckk+1:end,6*(kk-1)+7+20:6*(kk-1)+7+2+20) = sparse(dxrdomckk); + Jkk(2*Nckk+1:end,6*(kk-1)+7+3+20:6*(kk-1)+7+5+20) = sparse(dxrdTckk); + + Jkk(2*Nckk+1:end,11:12) = sparse(dxrdfr); + Jkk(2*Nckk+1:end,13:14) = sparse(dxrdcr); + Jkk(2*Nckk+1:end,15) = sparse(dxrdalphar); + Jkk(2*Nckk+1:end,16:20) = sparse(dxrdkr); + + + emax = max(abs(ekk)); + + if iter == 1; + e_ref = [e_ref;ekk]; + end; + + + if emax < threshold, + + J = [J;Jkk]; + e = [e;ekk]; + + else + + fprintf(1,'Disabling view %d - Reason: the left and right images are found inconsistent (try help calib_stereo for more information)\n',kk); + + active_images(kk) = 0; + active_images_left(kk) = 0; + active_images_right(kk) = 0; + + end; + + else + + param = [param;NaN*ones(6,1)]; + + end; + + end; + + history = [history param]; + + ind_Jac = find([est_fc_left_st & [1;est_aspect_ratio_left_st];center_optim_left_st*ones(2,1);est_alpha_left_st;est_dist_left_st;est_fc_right_st & [1;est_aspect_ratio_right_st];center_optim_right_st*ones(2,1);est_alpha_right_st;est_dist_right_st;ones(6,1);reshape(ones(6,1)*active_images,6*n_ima,1)]); + + ind_active = find(active_images); + + J = J(:,ind_Jac); + J2 = J'*J; + J2_inv = inv(J2); + + param_update = J2_inv*J'*e; + + + param(ind_Jac) = param(ind_Jac) + param_update; + + fc_left = param(1:2); + cc_left = param(3:4); + alpha_c_left = param(5); + kc_left = param(6:10); + fc_right = param(11:12); + cc_right = param(13:14); + alpha_c_right = param(15); + kc_right = param(16:20); + + + if ~est_aspect_ratio_left_st, + fc_left(2) = fc_left(1); + end; + if ~est_aspect_ratio_right_st, + fc_right(2) = fc_right(1); + end; + + om_old = om; + T_old = T; + + om = param(1+20:3+20); + T = param(4+20:6+20); + + + for kk = 1:n_ima; + + if active_images(kk), + + omckk = param(6*(kk-1)+7+20:6*(kk-1)+7+2+20); + Tckk = param(6*(kk-1)+7+3+20:6*(kk-1)+7+5+20); + + eval(['omc_left_' num2str(kk) ' = omckk;']); + eval(['Tc_left_' num2str(kk) ' = Tckk;']); + + end; + + end; + + change = norm([T;om] - [T_old;om_old])/norm([T;om]); + iter = iter + 1; + +end; + +fprintf(1,'done\n'); + + +history = [history param]; + +inconsistent_images = ~active_images & (active_images_left & active_images_right); + + +%%%--------------------------- Computation of the error of estimation: + +fprintf(1,'Estimation of uncertainties...'); + + +sigma_x = std(e(:)); +param_error = zeros(20 + (1+n_ima)*6,1); +param_error(ind_Jac) = 3*sqrt(full(diag(J2_inv)))*sigma_x; + +for kk = 1:n_ima; + + if active_images(kk), + + omckk_error = param_error(6*(kk-1)+7+20:6*(kk-1)+7+2+20); + Tckk = param_error(6*(kk-1)+7+3+20:6*(kk-1)+7+5+20); + + eval(['omc_left_error_' num2str(kk) ' = omckk;']); + eval(['Tc_left_error_' num2str(kk) ' = Tckk;']); + + else + + eval(['omc_left_' num2str(kk) ' = NaN*ones(3,1);']); + eval(['Tc_left_' num2str(kk) ' = NaN*ones(3,1);']); + eval(['omc_left_error_' num2str(kk) ' = NaN*ones(3,1);']); + eval(['Tc_left_error_' num2str(kk) ' = NaN*ones(3,1);']); + + end; + +end; + +fc_left_error = param_error(1:2); +cc_left_error = param_error(3:4); +alpha_c_left_error = param_error(5); +kc_left_error = param_error(6:10); +fc_right_error = param_error(11:12); +cc_right_error = param_error(13:14); +alpha_c_right_error = param_error(15); +kc_right_error = param_error(16:20); + +if ~est_aspect_ratio_left_st, + fc_left_error(2) = fc_left_error(1); +end; +if ~est_aspect_ratio_right_st, + fc_right_error(2) = fc_right_error(1); +end; + + +om_error = param_error(1+20:3+20); +T_error = param_error(4+20:6+20); + + +KK_left = [fc_left(1) fc_left(1)*alpha_c_left cc_left(1);0 fc_left(2) cc_left(2); 0 0 1]; +KK_right = [fc_right(1) fc_right(1)*alpha_c_right cc_right(1);0 fc_right(2) cc_right(2); 0 0 1]; + + +R = rodrigues(om); + +fprintf(1,'done\n'); + +fprintf(1,'\n\n\nStereo calibration parameters after optimization:\n'); + + +fprintf(1,'\n\nIntrinsic parameters of left camera:\n\n'); +fprintf(1,'Focal Length: fc_left = [ %3.5f %3.5f ] ± [ %3.5f %3.5f ]\n',[fc_left;fc_left_error]); +fprintf(1,'Principal point: cc_left = [ %3.5f %3.5f ] ± [ %3.5f %3.5f ]\n',[cc_left;cc_left_error]); +fprintf(1,'Skew: alpha_c_left = [ %3.5f ] ± [ %3.5f ] => angle of pixel axes = %3.5f ± %3.5f degrees\n',[alpha_c_left;alpha_c_left_error],90 - atan(alpha_c_left)*180/pi,atan(alpha_c_left_error)*180/pi); +fprintf(1,'Distortion: kc_left = [ %3.5f %3.5f %3.5f %3.5f %5.5f ] ± [ %3.5f %3.5f %3.5f %3.5f %5.5f ]\n',[kc_left;kc_left_error]); + + +fprintf(1,'\n\nIntrinsic parameters of right camera:\n\n'); +fprintf(1,'Focal Length: fc_right = [ %3.5f %3.5f ] ± [ %3.5f %3.5f ]\n',[fc_right;fc_right_error]); +fprintf(1,'Principal point: cc_right = [ %3.5f %3.5f ] ± [ %3.5f %3.5f ]\n',[cc_right;cc_right_error]); +fprintf(1,'Skew: alpha_c_right = [ %3.5f ] ± [ %3.5f ] => angle of pixel axes = %3.5f ± %3.5f degrees\n',[alpha_c_right;alpha_c_right_error],90 - atan(alpha_c_right)*180/pi,atan(alpha_c_right_error)*180/pi); +fprintf(1,'Distortion: kc_right = [ %3.5f %3.5f %3.5f %3.5f %5.5f ] ± [ %3.5f %3.5f %3.5f %3.5f %5.5f ]\n',[kc_right;kc_right_error]); + + +fprintf(1,'\n\nExtrinsic parameters (position of right camera wrt left camera):\n\n'); +fprintf(1,'Rotation vector: om = [ %3.5f %3.5f %3.5f ] ± [ %3.5f %3.5f %3.5f ]\n',[om;om_error]); +fprintf(1,'Translation vector: T = [ %3.5f %3.5f %3.5f ] ± [ %3.5f %3.5f %3.5f ]\n',[T;T_error]); + + +fprintf(1,'\n\nNote: The numerical errors are approximately three times the standard deviations (for reference).\n\n') +%fprintf(1,'\n\nSuggested threshold = %s\n\n',) + diff --git a/src/g_calib/ima_read_calib.m b/src/g_calib/ima_read_calib.m new file mode 100755 index 0000000000000000000000000000000000000000..da1ceea28bbb849c912f3ad0f771afeaed0ad856 --- /dev/null +++ b/src/g_calib/ima_read_calib.m @@ -0,0 +1,150 @@ + +if ~exist('calib_name')|~exist('format_image'), + data_calib; + return; +end; + +check_directory; + +if ~exist('n_ima'), + data_calib; + return; +end; + +check_active_images; + + +images_read = active_images; + + +if exist('image_numbers'), + first_num = image_numbers(1); +end; + + +% Just to fix a minor bug: +if ~exist('first_num'), + first_num = image_numbers(1); +end; + + +image_numbers = first_num:n_ima-1+first_num; + +no_image_file = 0; + +i = 1; + +while (i <= n_ima), % & (~no_image_file), + + if active_images(i), + + %fprintf(1,'Loading image %d...\n',i); + + if ~type_numbering, + number_ext = num2str(image_numbers(i)); + else + number_ext = sprintf(['%.' num2str(N_slots) 'd'],image_numbers(i)); + end; + + ima_name = [calib_name number_ext '.' format_image]; + + if i == ind_active(1), + fprintf(1,'Loading image '); + end; + + if exist(ima_name), + + fprintf(1,'%d...',i); + + if format_image(1) == 'p', + if format_image(2) == 'p', + Ii = double(loadppm(ima_name)); + else + Ii = double(loadpgm(ima_name)); + end; + else + if format_image(1) == 'r', + Ii = readras(ima_name); + else + Ii = double(imread(ima_name)); + end; + end; + + + if size(Ii,3)>1, + Ii = 0.299 * Ii(:,:,1) + 0.5870 * Ii(:,:,2) + 0.114 * Ii(:,:,3); + end; + + eval(['I_' num2str(i) ' = Ii;']); + + else + + %fprintf(1,'%d...no image...',i); + + images_read(i) = 0; + + %no_image_file = 1; + + end; + + end; + + i = i+1; + +end; + + +ind_read = find(images_read); + + + + +if isempty(ind_read), + + fprintf(1,'\nWARNING! No image were read\n'); + + no_image_file = 1; + + +else + + + %fprintf(1,'\nWARNING! Every exsisting image in the directory is set active.\n'); + + + if no_image_file, + + %fprintf(1,'WARNING! Some images were not read properly\n'); + + end; + + + fprintf(1,'\n'); + + if size(I_1,1)~=480, + small_calib_image = 1; + else + small_calib_image = 0; + end; + + [Hcal,Wcal] = size(I_1); % size of the calibration image + + [ny,nx] = size(I_1); + + clickname = []; + + map = gray(256); + + %string_save = 'save calib_data n_ima type_numbering N_slots image_numbers format_image calib_name Hcal Wcal nx ny map small_calib_image'; + + %eval(string_save); + + disp('done'); + %click_calib; + +end; + +if ~(exist('map')==1), map = gray(256); end; + +active_images = images_read; + diff --git a/src/g_calib/ima_read_calib_no_read.m b/src/g_calib/ima_read_calib_no_read.m new file mode 100755 index 0000000000000000000000000000000000000000..c4a71023333afd65888e7a5ba17c6d293526d849 --- /dev/null +++ b/src/g_calib/ima_read_calib_no_read.m @@ -0,0 +1,130 @@ + +if ~exist('calib_name')|~exist('format_image'), + data_calib_no_read; + return; +end; + +check_directory; + +if ~exist('n_ima'), + data_calib_no_read; + return; +end; + +check_active_images; + + +images_read = active_images; + + +if exist('image_numbers'), + first_num = image_numbers(1); +end; + + +% Just to fix a minor bug: +if ~exist('first_num'), + first_num = image_numbers(1); +end; + + +image_numbers = first_num:n_ima-1+first_num; + + +no_image_file = 0; + +% Step used to clean the memory if a previous atttempt has been made to read the entire set of images into memory: +for kk = 1:n_ima, + if (exist(['I_' num2str(kk)])==1), + clear(['I_' num2str(kk)]); + end; +end; + + +fprintf(1,'\nChecking directory content for the calibration images (no global image loading in memory efficient mode)\n'); + +one_image_read = 0; + +i = 1; + +while (i <= n_ima), % & (~no_image_file), + + if active_images(i), + + %fprintf(1,'Loading image %d...\n',i); + + if ~type_numbering, + number_ext = num2str(image_numbers(i)); + else + number_ext = sprintf(['%.' num2str(N_slots) 'd'],image_numbers(i)); + end; + + ima_name = [calib_name number_ext '.' format_image]; + + if i == ind_active(1), + fprintf(1,'Found images: '); + end; + + if exist(ima_name), + + fprintf(1,'%d...',i); + + + if ~one_image_read + + if format_image(1) == 'p', + if format_image(2) == 'p', + I = double(loadppm(ima_name)); + else + I = double(loadpgm(ima_name)); + end; + else + if format_image(1) == 'r', + I = readras(ima_name); + else + I = double(imread(ima_name)); + end; + end; + + + if size(I,3)>1, + I = 0.299 * I(:,:,1) + 0.5870 * I(:,:,2) + 0.114 * I(:,:,3); + end; + + + if size(I,1)~=480, + small_calib_image = 1; + else + small_calib_image = 0; + end; + + [Hcal,Wcal] = size(I); % size of the calibration image + + [ny,nx] = size(I); + + one_image_read = 1; + + end; + + + else + + images_read(i) = 0; + + end; + + end; + + i = i+1; + +end; + + +ind_read = find(images_read); + + +if ~(exist('map')==1), map = gray(256); end; + +active_images = images_read; + +fprintf(1,'\ndone\n'); diff --git a/src/g_calib/init_intrinsic_param.m b/src/g_calib/init_intrinsic_param.m new file mode 100755 index 0000000000000000000000000000000000000000..ad6ab23bb9e5fc3c1e88eb7caab9b46e040441a4 --- /dev/null +++ b/src/g_calib/init_intrinsic_param.m @@ -0,0 +1,187 @@ +%init_intrinsic_param +% +%Initialization of the intrinsic parameters. +%Runs as a script. +% +%INPUT: x_1,x_2,x_3,...: Feature locations on the images +% X_1,X_2,X_3,...: Corresponding grid coordinates +% +%OUTPUT: fc: Camera focal length +% cc: Principal point coordinates +% kc: Distortion coefficients +% alpha_c: skew coefficient +% KK: The camera matrix (containing fc, cc and alpha_c) +% +%Method: Computes the planar homographies H_1, H_2, H_3, ... and computes +% the focal length fc from orthogonal vanishing points constraint. +% The principal point cc is assumed at the center of the image. +% Assumes no image distortion (kc = [0;0;0;0]) +% +%Note: The row vector active_images consists of zeros and ones. To deactivate an image, set the +% corresponding entry in the active_images vector to zero. +% +% +%Important function called within that program: +% +%compute_homography.m: Computes the planar homography between points on the grid in 3D, and the image plane. +% +% +%VERY IMPORTANT: This function works only with 2D rigs. +%In the future, a more general function will be there (working with 3D rigs as well). + + +if ~exist('two_focals_init'), + two_focals_init = 0; +end; + +if ~exist('est_aspect_ratio'), + est_aspect_ratio = 1; +end; + +check_active_images; + +if ~exist(['x_' num2str(ind_active(1)) ]), + click_calib; +end; + + +fprintf(1,'\nInitialization of the intrinsic parameters - Number of images: %d\n',length(ind_active)); + + +% Initialize the homographies: + +for kk = 1:n_ima, + eval(['x_kk = x_' num2str(kk) ';']); + eval(['X_kk = X_' num2str(kk) ';']); + if (isnan(x_kk(1,1))), + if active_images(kk), + fprintf(1,'WARNING: Cannot calibrate with image %d. Need to extract grid corners first.\n',kk) + fprintf(1,' Set active_images(%d)=1; and run Extract grid corners.\n',kk) + end; + active_images(kk) = 0; + end; + if active_images(kk), + eval(['H_' num2str(kk) ' = compute_homography(x_kk,X_kk(1:2,:));']); + else + eval(['H_' num2str(kk) ' = NaN*ones(3,3);']); + end; +end; + +check_active_images; + +% initial guess for principal point and distortion: + +if ~exist('nx'), [ny,nx] = size(I); end; + +c_init = [nx;ny]/2 - 0.5; % initialize at the center of the image +k_init = [0;0;0;0;0]; % initialize to zero (no distortion) + + + +% Compute explicitely the focal length using all the (mutually orthogonal) vanishing points +% note: The vanihing points are hidden in the planar collineations H_kk + +A = []; +b = []; + +% matrix that subtract the principal point: +Sub_cc = [1 0 -c_init(1);0 1 -c_init(2);0 0 1]; + +for kk=1:n_ima, + + if active_images(kk), + + eval(['Hkk = H_' num2str(kk) ';']); + + Hkk = Sub_cc * Hkk; + + % Extract vanishing points (direct and diagonals): + + V_hori_pix = Hkk(:,1); + V_vert_pix = Hkk(:,2); + V_diag1_pix = (Hkk(:,1)+Hkk(:,2))/2; + V_diag2_pix = (Hkk(:,1)-Hkk(:,2))/2; + + V_hori_pix = V_hori_pix/norm(V_hori_pix); + V_vert_pix = V_vert_pix/norm(V_vert_pix); + V_diag1_pix = V_diag1_pix/norm(V_diag1_pix); + V_diag2_pix = V_diag2_pix/norm(V_diag2_pix); + + a1 = V_hori_pix(1); + b1 = V_hori_pix(2); + c1 = V_hori_pix(3); + + a2 = V_vert_pix(1); + b2 = V_vert_pix(2); + c2 = V_vert_pix(3); + + a3 = V_diag1_pix(1); + b3 = V_diag1_pix(2); + c3 = V_diag1_pix(3); + + a4 = V_diag2_pix(1); + b4 = V_diag2_pix(2); + c4 = V_diag2_pix(3); + + A_kk = [a1*a2 b1*b2; + a3*a4 b3*b4]; + + b_kk = -[c1*c2;c3*c4]; + + + A = [A;A_kk]; + b = [b;b_kk]; + + end; + +end; + + +% use all the vanishing points to estimate focal length: + + +% Select the model for the focal. (solution to Gerd's problem) +if ~two_focals_init + if b'*(sum(A')') < 0, + two_focals_init = 1; + end; +end; + + + +if two_focals_init + % Use a two focals estimate: + f_init = sqrt(abs(1./(inv(A'*A)*A'*b))); % if using a two-focal model for initial guess +else + % Use a single focal estimate: + f_init = sqrt(b'*(sum(A')') / (b'*b)) * ones(2,1); % if single focal length model is used +end; + + +if ~est_aspect_ratio, + f_init(1) = (f_init(1)+f_init(2))/2; + f_init(2) = f_init(1); +end; + +alpha_init = 0; + +%f_init = sqrt(b'*(sum(A')') / (b'*b)) * ones(2,1); % if single focal length model is used + + +% Global calibration matrix (initial guess): + +KK = [f_init(1) alpha_init*f_init(1) c_init(1);0 f_init(2) c_init(2); 0 0 1]; +inv_KK = inv(KK); + + +cc = c_init; +fc = f_init; +kc = k_init; +alpha_c = alpha_init; + + +fprintf(1,'\n\nCalibration parameters after initialization:\n\n'); +fprintf(1,'Focal Length: fc = [ %3.5f %3.5f ]\n',fc); +fprintf(1,'Principal point: cc = [ %3.5f %3.5f ]\n',cc); +fprintf(1,'Skew: alpha_c = [ %3.5f ] => angle of pixel = %3.5f degrees\n',alpha_c,90 - atan(alpha_c)*180/pi); +fprintf(1,'Distortion: kc = [ %3.5f %3.5f %3.5f %3.5f %5.5f ]\n',kc); diff --git a/src/g_calib/init_intrinsic_param_fisheye.m b/src/g_calib/init_intrinsic_param_fisheye.m new file mode 100755 index 0000000000000000000000000000000000000000..203326b1608e70ad9e27134dc27ab5eb25a7e520 --- /dev/null +++ b/src/g_calib/init_intrinsic_param_fisheye.m @@ -0,0 +1,82 @@ +%init_intrinsic_param_fisheye +% +%Initialization of the intrinsic parameters. +%Runs as a script. +% +%INPUT: x_1,x_2,x_3,...: Feature locations on the images +% X_1,X_2,X_3,...: Corresponding grid coordinates +% +%OUTPUT: fc: Camera focal length +% cc: Principal point coordinates +% kc: Fisheye distortion coefficients +% alpha_c: skew coefficient +% KK: The camera matrix (containing fc, cc and alpha_c) +% +%Method: Computes the planar homographies H_1, H_2, H_3, ... and computes +% the focal length fc from orthogonal vanishing points constraint. +% The principal point cc is assumed at the center of the image. +% Assumes no image distortion (kc = [0;0;0;0]) +% +%Note: The row vector active_images consists of zeros and ones. To deactivate an image, set the +% corresponding entry in the active_images vector to zero. +% +% +%Important function called within that program: +% +%compute_homography.m: Computes the planar homography between points on the grid in 3D, and the image plane. +% +% +%VERY IMPORTANT: This function works only with 2D rigs. +%In the future, a more general function will be there (working with 3D rigs as well). + + +if ~exist('two_focals_init'), + two_focals_init = 0; +end; + +if ~exist('est_aspect_ratio'), + est_aspect_ratio = 1; +end; + +check_active_images; + +if ~exist(['x_' num2str(ind_active(1)) ]), + click_calib; +end; + + +fprintf(1,'\nInitialization of the intrinsic parameters - Number of images: %d\n',length(ind_active)); + +check_active_images; + +% initial guess for principal point and distortion: + +if ~exist('nx'), [ny,nx] = size(I); end; + +f_init = (max(nx,ny) / pi) * ones(2,1); +c_init = [nx;ny]/2 - 0.5; % initialize at the center of the image +k_init = [0;0;0;0]; % initialize to zero (no distortion) + +if ~est_aspect_ratio, + f_init(1) = (f_init(1)+f_init(2))/2; + f_init(2) = f_init(1); +end; + +alpha_init = 0; + +% Global calibration matrix (initial guess): + +KK = [f_init(1) alpha_init*f_init(1) c_init(1);0 f_init(2) c_init(2); 0 0 1]; +inv_KK = inv(KK); + +cc = c_init; +fc = f_init; +kc = k_init; +alpha_c = alpha_init; + + +fprintf(1,'\n\nCalibration parameters after initialization:\n\n'); +fprintf(1,'Focal Length: fc = [ %3.5f %3.5f ]\n',fc); +fprintf(1,'Principal point: cc = [ %3.5f %3.5f ]\n',cc); +fprintf(1,'Skew: alpha_c = [ %3.5f ] => angle of pixel = %3.5f degrees\n',alpha_c,90 - atan(alpha_c)*180/pi); +fprintf(1,'Fisheye Distortion: kc = [ %3.5f %3.5f %3.5f %3.5f ]\n',kc); diff --git a/src/g_calib/inverse_motion.m b/src/g_calib/inverse_motion.m new file mode 100755 index 0000000000000000000000000000000000000000..5d605e25960ab85ea0ebd47d6fe43a8c840b8ced --- /dev/null +++ b/src/g_calib/inverse_motion.m @@ -0,0 +1,45 @@ +function [om2,T2,dom2dom,dom2dT,dT2dom,dT2dT] = inverse_motion(om,T); + +% This function computes the inverse motion corresponding to (om,T) + + +om2 = -om; +dom2dom = -eye(3); +dom2dT = zeros(3,3); + + +[R,dRdom] = rodrigues(om); +Rinv = R'; +dRinvdR = zeros(9,9); +dRinvdR([1 4 7],[1 2 3]) = eye(3); +dRinvdR([2 5 8],[4 5 6]) = eye(3); +dRinvdR([3 6 9],[7 8 9]) = eye(3); +dRinvdom = dRinvdR * dRdom; + +Tt = Rinv * T; +[dTtdRinv,dTtdT] = dAB(Rinv,T); + +T2 = -Tt; + +dT2dom = - dTtdRinv * dRinvdom; +dT2dT = - dTtdT; + + +return; + +% Test of the Jacobians: + +om = randn(3,1); +T = 10*randn(3,1); +[om2,T2,dom2dom,dom2dT,dT2dom,dT2dT] = inverse_motion(om,T); + +dom = randn(3,1) / 100000; +dT = randn(3,1) / 100000; + +[om3r,T3r] = inverse_motion(om+dom,T+dT); + +om3p = om2 + dom2dom*dom + dom2dT*dT; +T3p = T2 + dT2dom*dom + dT2dT*dT; + +%norm(om3r - om2) / norm(om3r - om3p) %-> Leads to infinity, since the opreation is linear! +norm(T3r - T2) / norm(T3r - T3p) diff --git a/src/g_calib/is3D.m b/src/g_calib/is3D.m new file mode 100755 index 0000000000000000000000000000000000000000..ab00b3db2b3d55ea8862c03a84a96efe816d6cb3 --- /dev/null +++ b/src/g_calib/is3D.m @@ -0,0 +1,19 @@ +function test = is3D(X), + + +Np = size(X,2); + +%% Check for planarity of the structure: + +X_mean = mean(X')'; + +Y = X - (X_mean*ones(1,Np)); + +YY = Y*Y'; + +[U,S,V] = svd(YY); + +r = S(3,3)/S(2,2); + +test = (r > 1e-3); + diff --git a/src/g_calib/load_image.m b/src/g_calib/load_image.m new file mode 100755 index 0000000000000000000000000000000000000000..9797f212eaba88a697d110145fcb757d90709c76 --- /dev/null +++ b/src/g_calib/load_image.m @@ -0,0 +1,41 @@ +function I = load_image(kk , calib_name , format_image , type_numbering , image_numbers , N_slots), + + +if ~type_numbering, + number_ext = num2str(image_numbers(kk)); +else + number_ext = sprintf(['%.' num2str(N_slots) 'd'],image_numbers(kk)); +end; + + +ima_name = [calib_name number_ext '.' format_image]; + +if ~exist(ima_name), + + fprintf(1,'Image %s not found!!!\n',ima_name); + I = NaN; + +else + + fprintf(1,'Loading image %s...\n',ima_name); + + if format_image(1) == 'p', + if format_image(2) == 'p', + I = double(loadppm(ima_name)); + else + I = double(loadpgm(ima_name)); + end; + else + if format_image(1) == 'r', + I = readras(ima_name); + else + I = double(imread(ima_name)); + end; + end; + + + if size(I,3)>1, + I = 0.299 * I(:,:,1) + 0.5870 * I(:,:,2) + 0.114 * I(:,:,3); + end; + +end; diff --git a/src/g_calib/load_stereo_calib_files.m b/src/g_calib/load_stereo_calib_files.m new file mode 100755 index 0000000000000000000000000000000000000000..3618186b6faa26182c4cc7de74628363c54f669f --- /dev/null +++ b/src/g_calib/load_stereo_calib_files.m @@ -0,0 +1,245 @@ + +dir('*mat'); + +fprintf(1,'Loading of the individual left and right camera calibration files\n'); + +calib_file_name_left = input('Name of the left camera calibration file ([]=Calib_Results_left.mat): ','s'); + +if isempty(calib_file_name_left), + calib_file_name_left = 'Calib_Results_left.mat'; +end; + + +calib_file_name_right = input('Name of the right camera calibration file ([]=Calib_Results_right.mat): ','s'); + +if isempty(calib_file_name_right), + calib_file_name_right = 'Calib_Results_right.mat'; +end; + + +if (exist(calib_file_name_left)~=2)|(exist(calib_file_name_right)~=2), + fprintf(1,'Error: left and right calibration files do not exist.\n'); + return; +end; + + +fprintf(1,'Loading the left camera calibration result file %s...\n',calib_file_name_left); + +clear calib_name + +load(calib_file_name_left); + +fc_left = fc; +cc_left = cc; +kc_left = kc; +alpha_c_left = alpha_c; +fc_left_error = fc_error; +cc_left_error = cc_error; +kc_left_error = kc_error; +alpha_c_left_error = alpha_c_error; +KK_left = KK; + +if exist('calib_name'), + calib_name_left = calib_name; + format_image_left = format_image; + type_numbering_left = type_numbering; + image_numbers_left = image_numbers; + N_slots_left = N_slots; +else + calib_name_left = ''; + format_image_left = ''; + type_numbering_left = ''; + image_numbers_left = ''; + N_slots_left = ''; +end; + + +X_left = []; + + +om_left_list = []; +T_left_list = []; + +for kk = 1:n_ima, + + if active_images(kk), + + eval(['Xkk = X_' num2str(kk) ';']); + eval(['omckk = omc_' num2str(kk) ';']); + eval(['Rckk = Rc_' num2str(kk) ';']); + eval(['Tckk = Tc_' num2str(kk) ';']); + + N = size(Xkk,2); + + Xckk = Rckk * Xkk + Tckk*ones(1,N); + + X_left = [X_left Xckk]; + + om_left_list = [om_left_list omckk]; + + T_left_list = [T_left_list Tckk]; + + end; +end; + + + +fprintf(1,'Loading the right camera calibration result file %s...\n',calib_file_name_right); + +clear calib_name + +load(calib_file_name_right); + +fc_right = fc; +cc_right = cc; +kc_right = kc; +alpha_c_right = alpha_c; +KK_right = KK; +fc_right_error = fc_error; +cc_right_error = cc_error; +kc_right_error = kc_error; +alpha_c_right_error = alpha_c_error; + +if exist('calib_name'), + calib_name_right = calib_name; + format_image_right = format_image; + type_numbering_right = type_numbering; + image_numbers_right = image_numbers; + N_slots_right = N_slots; +else + calib_name_right = ''; + format_image_right = ''; + type_numbering_right = ''; + image_numbers_right = ''; + N_slots_right = ''; +end; + +X_right = []; + +om_right_list = []; +T_right_list = []; + + +for kk = 1:n_ima, + + if active_images(kk), + + eval(['Xkk = X_' num2str(kk) ';']); + eval(['omckk = omc_' num2str(kk) ';']); + eval(['Rckk = Rc_' num2str(kk) ';']); + eval(['Tckk = Tc_' num2str(kk) ';']); + + N = size(Xkk,2); + + Xckk = Rckk * Xkk + Tckk*ones(1,N); + + X_right = [X_right Xckk]; + + om_right_list = [om_right_list omckk]; + T_right_list = [T_right_list Tckk]; + + end; +end; + + + + +om_ref_list = []; +T_ref_list = []; +for ii = 1:size(om_left_list,2), + % Align the structure from the first view: + R_ref = rodrigues(om_right_list(:,ii)) * rodrigues(om_left_list(:,ii))'; + T_ref = T_right_list(:,ii) - R_ref * T_left_list(:,ii); + om_ref = rodrigues(R_ref); + om_ref_list = [om_ref_list om_ref]; + T_ref_list = [T_ref_list T_ref]; +end; + + +% Robust estimate of the initial value for rotation and translation between the two views: +om = median(om_ref_list,2); +T = median(T_ref_list,2); + + + + +if 0, + figure(10); + plot3(X_right(1,:),X_right(2,:),X_right(3,:),'bo'); + hold on; + [Xr2] = rigid_motion(X_left,om,T); + plot3(Xr2(1,:),Xr2(2,:),Xr2(3,:),'r+'); + hold off; + drawnow; +end; + + +R = rodrigues(om); + + + +% Re-optimize now over all the set of extrinsic unknows (global optimization) and intrinsic parameters: + +load(calib_file_name_left); % Calib_Results_left; + +for kk = 1:n_ima, + if active_images(kk), + eval(['X_left_' num2str(kk) ' = X_' num2str(kk) ';']); + eval(['x_left_' num2str(kk) ' = x_' num2str(kk) ';']); + eval(['omc_left_' num2str(kk) ' = omc_' num2str(kk) ';']); + eval(['Rc_left_' num2str(kk) ' = Rc_' num2str(kk) ';']); + eval(['Tc_left_' num2str(kk) ' = Tc_' num2str(kk) ';']); + end; +end; + +center_optim_left = center_optim; +est_alpha_left = est_alpha; +est_dist_left = est_dist; +est_fc_left = est_fc; +est_aspect_ratio_left = est_aspect_ratio; +active_images_left = active_images; + + +load(calib_file_name_right); + +for kk = 1:n_ima, + if active_images(kk), + eval(['X_right_' num2str(kk) ' = X_' num2str(kk) ';']); + eval(['x_right_' num2str(kk) ' = x_' num2str(kk) ';']); + end; +end; + +center_optim_right = center_optim; +est_alpha_right = est_alpha; +est_dist_right = est_dist; +est_fc_right = est_fc; +est_aspect_ratio_right = est_aspect_ratio; +active_images_right = active_images; + + +active_images = active_images_left & active_images_right; + + + +fprintf(1,'\n\n\nStereo calibration parameters after loading the individual calibration files:\n'); + + +fprintf(1,'\n\nIntrinsic parameters of left camera:\n\n'); +fprintf(1,'Focal Length: fc_left = [ %3.5f %3.5f ] ± [ %3.5f %3.5f ]\n',[fc_left;fc_left_error]); +fprintf(1,'Principal point: cc_left = [ %3.5f %3.5f ] ± [ %3.5f %3.5f ]\n',[cc_left;cc_left_error]); +fprintf(1,'Skew: alpha_c_left = [ %3.5f ] ± [ %3.5f ] => angle of pixel axes = %3.5f ± %3.5f degrees\n',[alpha_c_left;alpha_c_left_error],90 - atan(alpha_c_left)*180/pi,atan(alpha_c_left_error)*180/pi); +fprintf(1,'Distortion: kc_left = [ %3.5f %3.5f %3.5f %3.5f %5.5f ] ± [ %3.5f %3.5f %3.5f %3.5f %5.5f ]\n',[kc_left;kc_left_error]); + + +fprintf(1,'\n\nIntrinsic parameters of right camera:\n\n'); +fprintf(1,'Focal Length: fc_right = [ %3.5f %3.5f ] ± [ %3.5f %3.5f ]\n',[fc_right;fc_right_error]); +fprintf(1,'Principal point: cc_right = [ %3.5f %3.5f ] ± [ %3.5f %3.5f ]\n',[cc_right;cc_right_error]); +fprintf(1,'Skew: alpha_c_right = [ %3.5f ] ± [ %3.5f ] => angle of pixel axes = %3.5f ± %3.5f degrees\n',[alpha_c_right;alpha_c_right_error],90 - atan(alpha_c_right)*180/pi,atan(alpha_c_right_error)*180/pi); +fprintf(1,'Distortion: kc_right = [ %3.5f %3.5f %3.5f %3.5f %5.5f ] ± [ %3.5f %3.5f %3.5f %3.5f %5.5f ]\n',[kc_right;kc_right_error]); + + +fprintf(1,'\n\nExtrinsic parameters (position of right camera wrt left camera):\n\n'); +fprintf(1,'Rotation vector: om = [ %3.5f %3.5f %3.5f ]\n',om); +fprintf(1,'Translation vector: T = [ %3.5f %3.5f %3.5f ]\n',T); + + diff --git a/src/g_calib/loading_calib.m b/src/g_calib/loading_calib.m new file mode 100755 index 0000000000000000000000000000000000000000..a0f50d2aa2d5ead7f00992b560916f6c83ad8ea1 --- /dev/null +++ b/src/g_calib/loading_calib.m @@ -0,0 +1,10 @@ +if ~exist('Calib_Results.mat'), + fprintf(1,'\nCalibration file Calib_Results.mat not found!\n'); + return; +end; + +fprintf(1,'\nLoading calibration results from Calib_Results.mat\n'); + +load Calib_Results + +fprintf(1,'done\n'); diff --git a/src/g_calib/loading_stereo_calib.m b/src/g_calib/loading_stereo_calib.m new file mode 100755 index 0000000000000000000000000000000000000000..018c9033f0150ba0d0ffd143aa0549be1fe3ef83 --- /dev/null +++ b/src/g_calib/loading_stereo_calib.m @@ -0,0 +1,7 @@ +if exist('Calib_Results_stereo.mat')~=2, + fprintf(1,'\nStereo calibration file Calib_Results_stereo.mat not found!\n'); + return; +end; + +fprintf(1,'Loading stereo calibration results from Calib_Results_stereo.mat...\n'); +load('Calib_Results_stereo.mat'); diff --git a/src/g_calib/loadinr.m b/src/g_calib/loadinr.m new file mode 100755 index 0000000000000000000000000000000000000000..91b6f8939a8a92973e463fdbf9940311ccfe0843 --- /dev/null +++ b/src/g_calib/loadinr.m @@ -0,0 +1,52 @@ +%LOADINR Load an INRIMAGE format file +% +% LOADINR(filename, im) +% +% Load an INRIA image format file and return it as a matrix +% +% SEE ALSO: saveinr +% +% Copyright (c) Peter Corke, 1999 Machine Vision Toolbox for Matlab + + +% Peter Corke 1996 + +function im = loadinr(fname, im) + + fid = fopen(fname, 'r'); + + s = fgets(fid); + if strcmp(s(1:12), '#INRIMAGE-4#') == 0, + error('not INRIMAGE format'); + end + + % not very complete, only looks for the X/YDIM keys + while 1, + s = fgets(fid); + n = length(s) - 1; + if s(1) == '#', + break + end + if strcmp(s(1:5), 'XDIM='), + cols = str2num(s(6:n)); + end + if strcmp(s(1:5), 'YDIM='), + rows = str2num(s(6:n)); + end + if strcmp(s(1:4), 'CPU='), + if strcmp(s(5:n), 'sun') == 0, + error('not sun data ordering'); + end + end + + end + disp(['INRIMAGE format file ' num2str(rows) ' x ' num2str(cols)]) + + % now the binary data + fseek(fid, 256, 'bof'); + [im count] = fread(fid, [cols rows], 'float32'); + im = im'; + if count ~= (rows*cols), + error('file too short'); + end + fclose(fid); diff --git a/src/g_calib/loadpgm.m b/src/g_calib/loadpgm.m new file mode 100755 index 0000000000000000000000000000000000000000..dfa8b611f98423fc6d1f286c3803b45bc0f0053e --- /dev/null +++ b/src/g_calib/loadpgm.m @@ -0,0 +1,89 @@ +%LOADPGM Load a PGM image +% +% I = loadpgm(filename) +% +% Returns a matrix containing the image loaded from the PGM format +% file filename. Handles ASCII (P2) and binary (P5) PGM file formats. +% +% If the filename has no extension, and open fails, a '.pgm' will +% be appended. +% +% +% Copyright (c) Peter Corke, 1999 Machine Vision Toolbox for Matlab + + +% Peter Corke 1994 + +function I = loadpgm(file) + white = [' ' 9 10 13]; % space, tab, lf, cr + white = setstr(white); + + fid = fopen(file, 'r'); + if fid < 0, + fid = fopen([file '.pgm'], 'r'); + end + if fid < 0, + error('Couldn''t open file'); + end + + magic = fread(fid, 2, 'char'); + while 1 + c = fread(fid,1,'char'); + if c == '#', + fgetl(fid); + elseif ~any(c == white) + fseek(fid, -1, 'cof'); % unputc() + break; + end + end + cols = fscanf(fid, '%d', 1); + while 1 + c = fread(fid,1,'char'); + if c == '#', + fgetl(fid); + elseif ~any(c == white) + fseek(fid, -1, 'cof'); % unputc() + break; + end + end + rows = fscanf(fid, '%d', 1); + while 1 + c = fread(fid,1,'char'); + if c == '#', + fgetl(fid); + elseif ~any(c == white) + fseek(fid, -1, 'cof'); % unputc() + break; + end + end + maxval = fscanf(fid, '%d', 1); + while 1 + c = fread(fid,1,'char'); + if c == '#', + fgetl(fid); + elseif ~any(c == white) + fseek(fid, -1, 'cof'); % unputc() + break; + end + end + if magic(1) == 'P', + if magic(2) == '2', + %disp(['ASCII PGM file ' num2str(rows) ' x ' num2str(cols)]) + I = fscanf(fid, '%d', [cols rows])'; + elseif magic(2) == '5', + %disp(['Binary PGM file ' num2str(rows) ' x ' num2str(cols)]) + if maxval == 1, + fmt = 'unint1'; + elseif maxval == 15, + fmt = 'uint4'; + elseif maxval == 255, + fmt = 'uint8'; + elseif maxval == 2^32-1, + fmt = 'uint32'; + end + I = fread(fid, [cols rows], fmt)'; + else + disp('Not a PGM file'); + end + end + fclose(fid); diff --git a/src/g_calib/loadppm.m b/src/g_calib/loadppm.m new file mode 100755 index 0000000000000000000000000000000000000000..c527b615e93e374ddd6f34fe7ee7a9d2b0169808 --- /dev/null +++ b/src/g_calib/loadppm.m @@ -0,0 +1,115 @@ +%LOADPPM Load a PPM image +% +% I = loadppm(filename) +% +% Returns a matrix containing the image loaded from the PPM format +% file filename. Handles ASCII (P3) and binary (P6) PPM file formats. +% +% If the filename has no extension, and open fails, a '.ppm' and +% '.pnm' extension will be tried. +% +% SEE ALSO: saveppm loadpgm +% +% Copyright (c) Peter Corke, 1999 Machine Vision Toolbox for Matlab + + +% Peter Corke 1994 + +function I = loadppm(file) + white = [' ' 9 10 13]; % space, tab, lf, cr + white = setstr(white); + + fid = fopen(file, 'r'); + if fid < 0, + fid = fopen([file '.ppm'], 'r'); + end + if fid < 0, + fid = fopen([file '.pnm'], 'r'); + end + if fid < 0, + error('Couldn''t open file'); + end + + magic = fread(fid, 2, 'char'); + while 1 + c = fread(fid,1,'char'); + if c == '#', + fgetl(fid); + elseif ~any(c == white) + fseek(fid, -1, 'cof'); % unputc() + break; + end + end + cols = fscanf(fid, '%d', 1); + while 1 + c = fread(fid,1,'char'); + if c == '#', + fgetl(fid); + elseif ~any(c == white) + fseek(fid, -1, 'cof'); % unputc() + break; + end + end + rows = fscanf(fid, '%d', 1); + while 1 + c = fread(fid,1,'char'); + if c == '#', + fgetl(fid); + elseif ~any(c == white) + fseek(fid, -1, 'cof'); % unputc() + break; + end + end + maxval = fscanf(fid, '%d', 1); + + % assume a carriage return only: + + c = fread(fid,1,'char'); + + % bug: because the image might be starting with special characters! + %while 1 + % c = fread(fid,1,'char'); + % if c == '#', + % fgetl(fid); + % elseif ~any(c == white) + % fseek(fid, -1, 'cof'); % unputc() + % break; + % end + %end + if magic(1) == 'P', + if magic(2) == '3', + %disp(['ASCII PPM file ' num2str(rows) ' x ' num2str(cols)]) + I = fscanf(fid, '%d', [cols*3 rows]); + elseif magic(2) == '6', + %disp(['Binary PPM file ' num2str(rows) ' x ' num2str(cols)]) + if maxval == 1, + fmt = 'unint1'; + elseif maxval == 15, + fmt = 'uint4'; + elseif maxval == 255, + fmt = 'uint8'; + elseif maxval == 2^32-1, + fmt = 'uint32'; + end + I = fread(fid, [cols*3 rows], fmt); + else + disp('Not a PPM file'); + end + end + % + % now the matrix has interleaved columns of R, G, B + % + I = I'; + size(I); + R = I(:,1:3:(cols*3)); + G = I(:,2:3:(cols*3)); + B = I(:,3:3:(cols*3)); + fclose(fid); + + + I = zeros(rows,cols,3); + I(:,:,1) = R; + I(:,:,2) = G; + I(:,:,3) = B; + I = uint8(I); + \ No newline at end of file diff --git a/src/g_calib/manual_corner_extraction.m b/src/g_calib/manual_corner_extraction.m new file mode 100755 index 0000000000000000000000000000000000000000..d3503d86c58aa2dfc896dd2131c5f88f7c980ee7 --- /dev/null +++ b/src/g_calib/manual_corner_extraction.m @@ -0,0 +1,141 @@ +%% This code allows complete manual reselection of every corner in the +%% images. +%% This tool is specifically useful in the case of highly distorted images. +%% +%% Use it when in standard mode. +%% In memory efficient mode, use manual_corner_extraction_no_read.m + + +if ~exist('n_ima'), + fprintf(1,'No image data available\n'); + return; +end; + +check_active_images; + +if n_ima == 0, + + fprintf(1,'No image data available\n'); + +else + +if ~exist(['I_' num2str(ind_active(1))]), + n_ima_save = n_ima; + active_images_save = active_images; + ima_read_calib; + n_ima = n_ima_save; + active_images = active_images_save; + check_active_images; + if no_image_file, + disp('Cannot extract corners without images'); + return; + end; +end; + +fprintf(1,'\nManual re-extraction of the grid corners on the images\n'); + +q_converge = input('Do you want to try to automatically find the closest corner? - only works with ckecker board corners ([]=yes, other = no) ','s'); + +if isempty(q_converge), + q_converge = 1; + fprintf(1,'Automatic refinement of the corner location after manual mouse click\n'); + disp('Window size for corner finder (wintx and winty):'); + wintx = input('wintx ([] = 5) = '); + if isempty(wintx), wintx = 5; end; + wintx = round(wintx); + winty = input('winty ([] = 5) = '); + if isempty(winty), winty = 5; end; + winty = round(winty); + + fprintf(1,'Window size = %dx%d\n',2*wintx+1,2*winty+1); +else + q_converge = 0; + fprintf(1,'No attempt to refine the corner location after manual mouse click\n'); +end; + + + + +ima_numbers = input('Number(s) of image(s) to process ([] = all images) = '); + +if isempty(ima_numbers), + ima_proc = 1:n_ima; +else + ima_proc = ima_numbers; +end; + +fprintf(1,'Processing image '); + +for kk = ima_proc; + + if active_images(kk), + + fprintf(1,'%d...',kk); + + eval(['I = I_' num2str(kk) ';']); + + eval(['x = x_' num2str(kk) ';']); + + Np = size(x,2); + + + figure(2); + image(I); + colormap(map); + hold on; + hx = plot(x(1,:)+1,x(2,:)+1,'r+'); + hcp = plot(x(1,1)+1,x(2,1)+1,'co'); + hold off; + + for np = 1:Np, + + set(hcp,'Xdata',x(1,np)+1,'Ydata',x(2,np)+1); + + + title(['Click on corner #' num2str(np) ' out of ' num2str(Np) ' (right button: keep point unchanged)']); + + [xi,yi,b] = ginput4(1); + + if b==1, + xxi = [xi;yi]; + if q_converge, + [xxi] = cornerfinder(xxi,I,winty,wintx); + end; + x(1,np) = xxi(1) - 1; + x(2,np) = xxi(2) - 1; + set(hx,'Xdata',x(1,:)+1,'Ydata',x(2,:)+1); + end; + + end; + + eval(['wintx_' num2str(kk) ' = wintx;']); + eval(['winty_' num2str(kk) ' = winty;']); + + eval(['x_' num2str(kk) '= x;']); + + else + + if ~exist(['omc_' num2str(kk)]), + + eval(['dX_' num2str(kk) ' = NaN;']); + eval(['dY_' num2str(kk) ' = NaN;']); + + eval(['wintx_' num2str(kk) ' = NaN;']); + eval(['winty_' num2str(kk) ' = NaN;']); + + eval(['x_' num2str(kk) ' = NaN*ones(2,1);']); + eval(['X_' num2str(kk) ' = NaN*ones(3,1);']); + + eval(['n_sq_x_' num2str(kk) ' = NaN;']); + eval(['n_sq_y_' num2str(kk) ' = NaN;']); + + end; + + end; + + +end; + +fprintf(1,'\ndone\n'); + +end; diff --git a/src/g_calib/manual_corner_extraction_no_read.m b/src/g_calib/manual_corner_extraction_no_read.m new file mode 100755 index 0000000000000000000000000000000000000000..b75d574fc71cf74818a2f8ef9097707b7dba16e2 --- /dev/null +++ b/src/g_calib/manual_corner_extraction_no_read.m @@ -0,0 +1,175 @@ +%% This code allows complete manual reselection of every corner in the +%% images. +%% This tool is specifically useful in the case of highly distorted images. +%% +%% Use it when in memory efficient mode. +%% In standard mode, use manual_corner_extraction.m + + +if ~exist('n_ima'), + fprintf(1,'No image data available\n'); + return; +end; + +check_active_images; + +if n_ima == 0, + + fprintf(1,'No image data available\n'); + +else + +%if ~exist(['I_' num2str(ind_active(1))]), +% n_ima_save = n_ima; +% active_images_save = active_images; +% ima_read_calib; +% n_ima = n_ima_save; +% active_images = active_images_save; +% check_active_images; +% if no_image_file, +% disp('Cannot extract corners without images'); +% return; +% end; +%end; + +fprintf(1,'\nManual re-extraction of the grid corners on the images\n'); + +q_converge = input('Do you want to try to automatically find the closest corner? - only works with ckecker board corners ([]=yes, other = no)','s'); + +if isempty(q_converge), + q_converge = 1; + fprintf(1,'Automatic refinement of the corner location after manual mouse click\n'); + disp('Window size for corner finder (wintx and winty):'); + wintx = input('wintx ([] = 5) = '); + if isempty(wintx), wintx = 5; end; + wintx = round(wintx); + winty = input('winty ([] = 5) = '); + if isempty(winty), winty = 5; end; + winty = round(winty); + + fprintf(1,'Window size = %dx%d\n',2*wintx+1,2*winty+1); +else + q_converge = 0; + fprintf(1,'No attempt to refine the corner location after manual mouse click\n'); +end; + + +ima_numbers = input('Number(s) of image(s) to process ([] = all images) = '); + +if isempty(ima_numbers), + ima_proc = 1:n_ima; +else + ima_proc = ima_numbers; +end; + +fprintf(1,'Processing image '); + +for kk = ima_proc; + + if active_images(kk), + + fprintf(1,'%d...',kk); + + if ~type_numbering, + number_ext = num2str(image_numbers(kk)); + else + number_ext = sprintf(['%.' num2str(N_slots) 'd'],image_numbers(kk)); + end; + + ima_name = [calib_name number_ext '.' format_image]; + + + if exist(ima_name), + + + if format_image(1) == 'p', + if format_image(2) == 'p', + I = double(loadppm(ima_name)); + else + I = double(loadpgm(ima_name)); + end; + else + if format_image(1) == 'r', + I = readras(ima_name); + else + I = double(imread(ima_name)); + end; + end; + + + if size(I,3)>1, + I = 0.299 * I(:,:,1) + 0.5870 * I(:,:,2) + 0.114 * I(:,:,3); + end; + + [ny,nx,junk] = size(I); + + eval(['x = x_' num2str(kk) ';']); + + Np = size(x,2); + + + figure(2); + image(I); + colormap(map); + hold on; + hx = plot(x(1,:)+1,x(2,:)+1,'r+'); + hcp = plot(x(1,1)+1,x(2,1)+1,'co'); + hold off; + + for np = 1:Np, + + set(hcp,'Xdata',x(1,np)+1,'Ydata',x(2,np)+1); + + + title(['Click on corner #' num2str(np) ' out of ' num2str(Np) ' (right button: keep point unchanged)']); + + [xi,yi,b] = ginput4(1); + + if b==1, + xxi = [xi;yi]; + if q_converge, + [xxi] = cornerfinder(xxi,I,winty,wintx); + end; + x(1,np) = xxi(1) - 1; + x(2,np) = xxi(2) - 1; + set(hx,'Xdata',x(1,:)+1,'Ydata',x(2,:)+1); + end; + + end; + + eval(['wintx_' num2str(kk) ' = wintx;']); + eval(['winty_' num2str(kk) ' = winty;']); + + eval(['x_' num2str(kk) '= x;']); + + else + fprintf(1,'Image %s not found!!!...',ima_name); + end; + + + else + + if ~exist(['omc_' num2str(kk)]), + + eval(['dX_' num2str(kk) ' = NaN;']); + eval(['dY_' num2str(kk) ' = NaN;']); + + eval(['wintx_' num2str(kk) ' = NaN;']); + eval(['winty_' num2str(kk) ' = NaN;']); + + eval(['x_' num2str(kk) ' = NaN*ones(2,1);']); + eval(['X_' num2str(kk) ' = NaN*ones(3,1);']); + + eval(['n_sq_x_' num2str(kk) ' = NaN;']); + eval(['n_sq_y_' num2str(kk) ' = NaN;']); + + end; + + end; + + +end; + +fprintf(1,'\ndone\n'); + +end; diff --git a/src/g_calib/mean_std_robust.m b/src/g_calib/mean_std_robust.m new file mode 100755 index 0000000000000000000000000000000000000000..0d18a625ccb634cce17b63318f57882c85dbb1d3 --- /dev/null +++ b/src/g_calib/mean_std_robust.m @@ -0,0 +1,7 @@ +function [m,s] = mean_std_robust(x); + +x = x(:); + +m = median(x); + +s = median(abs(x - m))*1.4836; diff --git a/src/g_calib/merge_calibration_sets.m b/src/g_calib/merge_calibration_sets.m new file mode 100755 index 0000000000000000000000000000000000000000..dc841903df94a6312e7e9e9a5d1295a6a71cdb32 --- /dev/null +++ b/src/g_calib/merge_calibration_sets.m @@ -0,0 +1,85 @@ + +set1 = load(data_set1); % part1\Calib_Results; +set2 = load(data_set2); % part2\Calib_Results; + +shift = set1.n_ima; + +for kk = 1:set1.n_ima + +eval(['X_' num2str(kk) ' = set1.X_' num2str(kk) ';']); + +eval(['dX_' num2str(kk) ' = set1.dX_' num2str(kk) ';']); +eval(['dY_' num2str(kk) ' = set1.dY_' num2str(kk) ';']); + +eval(['wintx_' num2str(kk) ' = set1.wintx_' num2str(kk) ';']); +eval(['winty_' num2str(kk) ' = set1.winty_' num2str(kk) ';']); + +eval(['x_' num2str(kk) ' = set1.x_' num2str(kk) ';']); + +if isfield(set1,'y') + eval(['y_' num2str(kk) ' = set1.y_' num2str(kk) ';']); +else + eval(['y_' num2str(kk) ' = [NaN];']); +end; + +eval(['n_sq_x_' num2str(kk) ' = set1.n_sq_x_' num2str(kk) ';']); +eval(['n_sq_y_' num2str(kk) ' = set1.n_sq_y_' num2str(kk) ';']); + + +if isfield(set1,['omc_' num2str(kk+shift)]) + eval(['omc_' num2str(kk+shift) ' = set1.omc_' num2str(kk) ';']); + eval(['Tc_' num2str(kk+shift) ' = set1.Tc_' num2str(kk) ';']); +else + eval(['omc_' num2str(kk+shift) ' = [NaN;NaN;NaN];']); + eval(['Tc_' num2str(kk+shift) ' = [NaN;NaN;NaN];']); +end; + +end; + +for kk = 1:set2.n_ima + +eval(['X_' num2str(kk+shift) ' = set2.X_' num2str(kk) ';']); + + +eval(['dX_' num2str(kk+shift) ' = set2.dX_' num2str(kk) ';']); +eval(['dY_' num2str(kk+shift) ' = set2.dY_' num2str(kk) ';']); + +eval(['wintx_' num2str(kk+shift) ' = set2.wintx_' num2str(kk) ';']); +eval(['winty_' num2str(kk+shift) ' = set2.winty_' num2str(kk) ';']); + +eval(['x_' num2str(kk+shift) ' = set2.x_' num2str(kk) ';']); + +if isfield(set2,'y') + eval(['y_' num2str(kk) ' = set2.y_' num2str(kk) ';']); +else + eval(['y_' num2str(kk) ' = [NaN];']); +end; + +eval(['n_sq_x_' num2str(kk+shift) ' = set2.n_sq_x_' num2str(kk) ';']); +eval(['n_sq_y_' num2str(kk+shift) ' = set2.n_sq_y_' num2str(kk) ';']); + + + +if isfield(set2,['omc_' num2str(kk+shift)]) + eval(['omc_' num2str(kk+shift) ' = set2.omc_' num2str(kk) ';']); + eval(['Tc_' num2str(kk+shift) ' = set2.Tc_' num2str(kk) ';']); +else + eval(['omc_' num2str(kk+shift) ' = [NaN;NaN;NaN];']); + eval(['Tc_' num2str(kk+shift) ' = [NaN;NaN;NaN];']); +end; + +end; + + +fc = set2.fc; +kc = set2.kc; +cc = set2.cc; +alpha_c = set2.alpha_c; +KK = set2.KK; +inv_KK = set2.inv_KK; + + +n_ima = set1.n_ima + set2.n_ima; +active_images = [set1.active_images set2.active_images]; + +no_image = 1; diff --git a/src/g_calib/merge_two_datasets.m b/src/g_calib/merge_two_datasets.m new file mode 100755 index 0000000000000000000000000000000000000000..dd3ac02dedaf416bc56ed807e070a29738031856 --- /dev/null +++ b/src/g_calib/merge_two_datasets.m @@ -0,0 +1,68 @@ +fprintf(1,'Script that merges two "Cabib_Results.mat" data sets of the same camera into a single dataset\n') + +dir; + + +cont = 1; +while cont + data_set1 = input('Filename of the first dataset (with complete path if necessary): ','s'); + cont = ((exist(data_set1)~=2)); + if cont, + fprintf(1,'File not found. Try again.\n'); + end; +end; +cont = 1; +while cont + data_set2 = input('Filename of the second dataset (with complete path if necessary): ','s'); + cont = ((exist(data_set2)~=2)); + if cont, + fprintf(1,'File not found. Try again.\n'); + end; +end; + + +load(data_set1); % part1\Calib_Results; + +shift = n_ima; + +load(data_set2); % part2\Calib_Results; + +active_images2 = active_images; +n_ima2 = n_ima; + + +for kk = 1:n_ima + +eval(['X_' num2str(kk+shift) ' = X_' num2str(kk) ';']); + + +eval(['dX_' num2str(kk+shift) ' = dX_' num2str(kk) ';']); +eval(['dY_' num2str(kk+shift) ' = dY_' num2str(kk) ';']); + +eval(['wintx_' num2str(kk+shift) ' = wintx_' num2str(kk) ';']); +eval(['winty_' num2str(kk+shift) ' = winty_' num2str(kk) ';']); + +eval(['x_' num2str(kk+shift) ' = x_' num2str(kk) ';']); +eval(['y_' num2str(kk+shift) ' = y_' num2str(kk) ';']); + +eval(['n_sq_x_' num2str(kk+shift) ' = n_sq_x_' num2str(kk) ';']); +eval(['n_sq_y_' num2str(kk+shift) ' = n_sq_y_' num2str(kk) ';']); + + +eval(['omc_' num2str(kk+shift) ' = omc_' num2str(kk) ';']); +eval(['Tc_' num2str(kk+shift) ' = Tc_' num2str(kk) ';']); + +end; + +load(data_set1); % part1\Calib_Results; + +n_ima = n_ima + n_ima2; +active_images = [active_images active_images2]; + +no_image = 1; + +% Recompute the error (in the vector ex): +comp_error_calib; + +fprintf('The two calibration datasets are now merged. You are now ready to run calibration. \n'); + diff --git a/src/g_calib/mosaic.m b/src/g_calib/mosaic.m new file mode 100755 index 0000000000000000000000000000000000000000..37c916c0c8bbbaa3b8a061e7a6490b3825f2a710 --- /dev/null +++ b/src/g_calib/mosaic.m @@ -0,0 +1,92 @@ + +if ~exist('I_1'), + active_images_save = active_images; + ima_read_calib; + active_images = active_images_save; + check_active_images; +end; + +check_active_images; + +if isempty(ind_read), + return; +end; + + +n_col = floor(sqrt(n_ima*nx/ny)); + +n_row = ceil(n_ima / n_col); + + +ker2 = 1; +for ii = 1:n_col, + ker2 = conv(ker2,[1/4 1/2 1/4]); +end; + + +II = I_1(1:n_col:end,1:n_col:end); + +[ny2,nx2] = size(II); + + + +kk_c = 1; + +II_mosaic = []; + +for jj = 1:n_row, + + + II_row = []; + + for ii = 1:n_col, + + if (exist(['I_' num2str(kk_c)])) & (kk_c <= n_ima), + + if active_images(kk_c), + eval(['I = I_' num2str(kk_c) ';']); + %I = conv2(conv2(I,ker2,'same'),ker2','same'); % anti-aliasing + I = I(1:n_col:end,1:n_col:end); + else + I = zeros(ny2,nx2); + end; + + else + + I = zeros(ny2,nx2); + + end; + + + + II_row = [II_row I]; + + if ii ~= n_col, + + II_row = [II_row zeros(ny2,3)]; + + end; + + + kk_c = kk_c + 1; + + end; + + nn2 = size(II_row,2); + + if jj ~= n_row, + II_row = [II_row; zeros(3,nn2)]; + end; + + II_mosaic = [II_mosaic ; II_row]; + +end; + +figure(2); +image(II_mosaic); +colormap(gray(256)); +title('Calibration images'); +set(gca,'Xtick',[]) +set(gca,'Ytick',[]) +axis('image'); + diff --git a/src/g_calib/mosaic_no_read.m b/src/g_calib/mosaic_no_read.m new file mode 100755 index 0000000000000000000000000000000000000000..ad013d862ec697ee85fe3d6b5d650180f41757df --- /dev/null +++ b/src/g_calib/mosaic_no_read.m @@ -0,0 +1,118 @@ + + +if ~exist('n_ima'), + data_calib_no_read; +end; + + +check_active_images; + +n_col = floor(sqrt(n_ima*nx/ny)); + +n_row = ceil(n_ima / n_col); + + +ker2 = 1; +for ii = 1:n_col, + ker2 = conv(ker2,[1/4 1/2 1/4]); +end; + +ny2 = length(1:n_col:ny); +nx2 = length(1:n_col:nx); + +%II = I_1(1:n_col:end,1:n_col:end); +%[ny2,nx2] = size(II); + + + +kk_c = 1; + +II_mosaic = []; + +for jj = 1:n_row, + + + II_row = []; + + for ii = 1:n_col, + + + if (kk_c <= n_ima), + + if ~type_numbering, + number_ext = num2str(image_numbers(kk_c)); + else + number_ext = sprintf(['%.' num2str(N_slots) 'd'],image_numbers(kk_c)); + end; + + ima_name = [calib_name number_ext '.' format_image]; + + + + if (exist(ima_name)) & (kk_c <= n_ima), + + if active_images(kk_c), + + if format_image(1) == 'p', + if format_image(2) == 'p', + I = double(loadppm(ima_name)); + else + I = double(loadpgm(ima_name)); + end; + else + if format_image(1) == 'r', + I = readras(ima_name); + else + I = double(imread(ima_name)); + end; + end; + + %I = conv2(conv2(I,ker2,'same'),ker2','same'); % anti-aliasing + I = I(1:n_col:end,1:n_col:end); + else + I = zeros(ny2,nx2); + end; + + else + + I = zeros(ny2,nx2); + + end; + + else + + I = zeros(ny2,nx2); + + end; + + II_row = [II_row I]; + + if ii ~= n_col, + + II_row = [II_row zeros(ny2,3)]; + + end; + + + kk_c = kk_c + 1; + + end; + + nn2 = size(II_row,2); + + if jj ~= n_row, + II_row = [II_row; zeros(3,nn2)]; + end; + + II_mosaic = [II_mosaic ; II_row]; + +end; + +figure(2); +image(II_mosaic); +colormap(gray(256)); +title('Calibration images'); +set(gca,'Xtick',[]) +set(gca,'Ytick',[]) +axis('image'); + diff --git a/src/g_calib/normalize.m b/src/g_calib/normalize.m new file mode 100755 index 0000000000000000000000000000000000000000..c293f86f19c248726eb9ba22acb10fe2992007e7 --- /dev/null +++ b/src/g_calib/normalize.m @@ -0,0 +1,47 @@ +function [xn] = normalize(x_kk,fc,cc,kc,alpha_c) + +%normalize +% +%[xn] = normalize(x_kk,fc,cc,kc,alpha_c) +% +%Computes the normalized coordinates xn given the pixel coordinates x_kk +%and the intrinsic camera parameters fc, cc and kc. +% +%INPUT: x_kk: Feature locations on the images +% fc: Camera focal length +% cc: Principal point coordinates +% kc: Distortion coefficients +% alpha_c: Skew coefficient +% +%OUTPUT: xn: Normalized feature locations on the image plane (a 2XN matrix) +% +%Important functions called within that program: +% +%comp_distortion_oulu: undistort pixel coordinates. + +if nargin < 5, + alpha_c = 0; + if nargin < 4; + kc = [0;0;0;0;0]; + if nargin < 3; + cc = [0;0]; + if nargin < 2, + fc = [1;1]; + end; + end; + end; +end; + + +% First: Subtract principal point, and divide by the focal length: +x_distort = [(x_kk(1,:) - cc(1))/fc(1);(x_kk(2,:) - cc(2))/fc(2)]; + +% Second: undo skew +x_distort(1,:) = x_distort(1,:) - alpha_c * x_distort(2,:); + +if norm(kc) ~= 0, + % Third: Compensate for lens distortion: + xn = comp_distortion_oulu(x_distort,kc); +else + xn = x_distort; +end; diff --git a/src/g_calib/normalize2.m b/src/g_calib/normalize2.m new file mode 100755 index 0000000000000000000000000000000000000000..d3d56043b0a3cbf82152e2947987d816dd50911e --- /dev/null +++ b/src/g_calib/normalize2.m @@ -0,0 +1,73 @@ +function [xn,dxdf,dxdc,dxdk,dxdalpha] = normalize2(x_kk,fc,cc,kc,alpha_c), + +%normalize +% +%[xn] = normalize(x_kk,fc,cc,kc,alpha_c) +% +%Computes the normalized coordinates xn given the pixel coordinates x_kk +%and the intrinsic camera parameters fc, cc and kc. +% +%INPUT: x_kk: Feature locations on the images +% fc: Camera focal length +% cc: Principal point coordinates +% kc: Distortion coefficients +% alpha_c: Skew coefficient +% +%OUTPUT: xn: Normalized feature locations on the image plane (a 2XN matrix) +% +%Important functions called within that program: + +k1 = kc(1); +k2 = kc(2); +k3 = kc(5); +p1 = kc(3); +p2 = kc(4); + +N = size(x_kk,2); + +% First: Subtract principal point, and divide by the focal length: +x_distort = [(x_kk(1,:) - cc(1))/fc(1);(x_kk(2,:) - cc(2))/fc(2)]; + + +v1 = - x_distort(1,:) / fc(1); +v2 = - x_distort(2,:) / fc(1); + +dx_distortdfc = zeros(2*N,2); +dx_distortdfc(1:2:end,1) = v1'; +dx_distortdfc(2:2:end,2) = v2'; + +v1 = - x_distort(1,:) / fc(1); +v2 = - x_distort(2,:) / fc(1); + +dx_distortdcc = zeros(2*N,2); +dx_distortdcc(1:2:end,1) = -(1/fc(1)) * ones(N,1); +dx_distortdcc(2:2:end,2) = -(1/fc(2)) * ones(N,1); + +% Second: undo skew +x_distort(1,:) = x_distort(1,:) - alpha_c * x_distort(2,:); + +dx_distort2dfc = [ dx_distortdfc(:,1)-alpha_c *dx_distortdfc(:,2) dx_distortdfc(:,2)]; +dx_distort2dcc = [ dx_distortdcc(:,1)-alpha_c *dx_distortdcc(:,2) dx_distortdcc(:,2)]; + +dx_distort2dalpha_c = zeros(2*N,1); +dx_distort2dalpha_c(1:2:end) = -x_distort(2,:)'; + +x = x_distort; % initial guess + +for kk=1:20, + + r_2 = sum(x.^2); + k_radial = 1 + k1 * r_2 + k2 * r_2.^2 + k3 * r_2.^3; + delta_x = [2*p1*x(1,:).*x(2,:) + p2*(r_2 + 2*x(1,:).^2); p1 * (r_2 + 2*x(2,:).^2)+2*p2*x(1,:).*x(2,:)]; + x = (x_distort - delta_x)./(ones(2,1)*k_radial); + +end; + + +xn = x; + + +dxdk = zeros(2*N,5); % Approximation (no time) +dxdf = dx_distort2dfc; +dxdc = dx_distort2dcc; +dxdalpha = dx_distort2dalpha_c; diff --git a/src/g_calib/normalize_pixel.m b/src/g_calib/normalize_pixel.m new file mode 100755 index 0000000000000000000000000000000000000000..467fe14f3744775f305ad556540ea8f9d917c8f5 --- /dev/null +++ b/src/g_calib/normalize_pixel.m @@ -0,0 +1,47 @@ +function [xn] = normalize_pixel(x_kk,fc,cc,kc,alpha_c) + +%normalize +% +%[xn] = normalize_pixel(x_kk,fc,cc,kc,alpha_c) +% +%Computes the normalized coordinates xn given the pixel coordinates x_kk +%and the intrinsic camera parameters fc, cc and kc. +% +%INPUT: x_kk: Feature locations on the images +% fc: Camera focal length +% cc: Principal point coordinates +% kc: Distortion coefficients +% alpha_c: Skew coefficient +% +%OUTPUT: xn: Normalized feature locations on the image plane (a 2XN matrix) +% +%Important functions called within that program: +% +%comp_distortion_oulu: undistort pixel coordinates. + +if nargin < 5, + alpha_c = 0; + if nargin < 4; + kc = [0;0;0;0;0]; + if nargin < 3; + cc = [0;0]; + if nargin < 2, + fc = [1;1]; + end; + end; + end; +end; + + +% First: Subtract principal point, and divide by the focal length: +x_distort = [(x_kk(1,:) - cc(1))/fc(1);(x_kk(2,:) - cc(2))/fc(2)]; + +% Second: undo skew +x_distort(1,:) = x_distort(1,:) - alpha_c * x_distort(2,:); + +if norm(kc) ~= 0, + % Third: Compensate for lens distortion: + xn = comp_distortion_oulu(x_distort,kc); +else + xn = x_distort; +end; diff --git a/src/g_calib/normalize_pixel_fisheye.m b/src/g_calib/normalize_pixel_fisheye.m new file mode 100755 index 0000000000000000000000000000000000000000..e3dd632366e23a0481908270ca6165907b098570 --- /dev/null +++ b/src/g_calib/normalize_pixel_fisheye.m @@ -0,0 +1,40 @@ +function [xn] = normalize_pixel_fisheye(x_kk,fc,cc,kc,alpha_c) + +%normalize +% +%[xn] = normalize_pixel(x_kk,fc,cc,kc,alpha_c) +% +%Computes the normalized coordinates xn given the pixel coordinates x_kk +%and the intrinsic camera parameters fc, cc and kc. +% +%INPUT: x_kk: Feature locations on the images +% fc: Camera focal length +% cc: Principal point coordinates +% kc: Fisheye distortion coefficients +% alpha_c: Skew coefficient +% +%OUTPUT: xn: Normalized feature locations on the image plane (a 2XN matrix) + +if nargin < 5, + alpha_c = 0; + if nargin < 4; + kc = [0;0;0;0;0]; + if nargin < 3; + cc = [0;0]; + if nargin < 2, + fc = [1;1]; + end; + end; + end; +end; + + +% First: Subtract principal point, and divide by the focal length: +x_distort = [(x_kk(1,:) - cc(1))/fc(1);(x_kk(2,:) - cc(2))/fc(2)]; + +% Second: undo skew +x_distort(1,:) = x_distort(1,:) - alpha_c * x_distort(2,:); + +% Third: Compensate for lens distortion: +xn = comp_fisheye_distortion(x_distort,kc); + diff --git a/src/g_calib/pgmread.m b/src/g_calib/pgmread.m new file mode 100755 index 0000000000000000000000000000000000000000..c96ccb71c76ca5846afdd30bfcfd41248e7a1609 --- /dev/null +++ b/src/g_calib/pgmread.m @@ -0,0 +1,26 @@ +function img = pgmread(filename) +% function img = pgmread(filename) +% this is my version of pgmread for the pgm file created by XV. +% +% this program also corrects for the shifts in the image from pm file. + +fid = fopen(filename,'r'); +fscanf(fid, 'P5\n'); +cmt = '#'; +while findstr(cmt, '#'), + cmt = fgets(fid); + if length(findstr(cmt, '#')) ~= 1, + YX = sscanf(cmt, '%d %d'); + y = YX(1); x = YX(2); + end +end + +%fgets(fid); + +%img = fscanf(fid,'%d',size); +%img = img'; + +img = fread(fid,[y,x],'uint8'); +img = img'; +fclose(fid); + diff --git a/src/g_calib/point_distribution.m b/src/g_calib/point_distribution.m new file mode 100755 index 0000000000000000000000000000000000000000..1b72409625c1a5f36d40954638dbe7aaf2a70585 --- /dev/null +++ b/src/g_calib/point_distribution.m @@ -0,0 +1,45 @@ +% Point Distribution + +colors = 'brgkcm'; + +if ~exist('n_ima')|~exist('fc'), + fprintf(1,'No calibration data available.\n'); + return; +end; + +check_active_images; + +if ~exist(['ex_' num2str(ind_active(1)) ]), + fprintf(1,'Need to calibrate before analysing reprojection error. Maybe need to load Calib_Results.mat file.\n'); + return; +end; + +figure(6); + +for kk=1:n_ima + + if exist(['x_' num2str(kk)]), + + if active_images(kk) & eval(['~isnan(x_' num2str(kk) '(1,1))']), + + eval(['plot(x_' num2str(kk) '(1,:),x_' num2str(kk) '(2,:),''' colors(rem(kk-1,6)+1) '+'');']); + + hold on; + end; + + end; + +end; + +axis('equal'); + +axis([0 nx 0 ny]); + +title1=pwd; +title1=strrep(title1,'_','\_'); + +title({'Point Distribution in Images',title1}); + +xlabel('x'); + +ylabel('y'); diff --git a/src/g_calib/project2_oulu.m b/src/g_calib/project2_oulu.m new file mode 100755 index 0000000000000000000000000000000000000000..c5c4a34b1b94b44ea0ac111f374edbb880ad436b --- /dev/null +++ b/src/g_calib/project2_oulu.m @@ -0,0 +1,53 @@ +function [x] = project2_oulu(X,R,T,f,t,k) +%PROJECT Subsidiary to calib + +% (c) Pietro Perona -- March 24, 1994 +% California Institute of Technology +% Pasadena, CA +% +% Renamed because project exists in matlab 5.2!!! +% Now uses the more elaborate intrinsic model from Oulu + + + +[m,n] = size(X); + +Y = R*X + T*ones(1,n); +Z = Y(3,:); + +f = f(:); %% make a column vector +if length(f)==1, + f = [f f]'; +end; + +x = (Y(1:2,:) ./ (ones(2,1) * Z)) ; + + +radius_2 = x(1,:).^2 + x(2,:).^2; + +if length(k) > 1, + + radial_distortion = 1 + ones(2,1) * ((k(1) * radius_2) + (k(2) * radius_2.^2)); + + if length(k) < 4, + + delta_x = zeros(2,n); + + else + + delta_x = [2*k(3)*x(1,:).*x(2,:) + k(4)*(radius_2 + 2*x(1,:).^2) ; + k(3) * (radius_2 + 2*x(2,:).^2)+2*k(4)*x(1,:).*x(2,:)]; + + end; + + +else + + radial_distortion = 1 + ones(2,1) * ((k(1) * radius_2)); + + delta_x = zeros(2,n); + +end; + + +x = (x .* radial_distortion + delta_x).* (f * ones(1,n)) + t*ones(1,n); diff --git a/src/g_calib/project_points.m b/src/g_calib/project_points.m new file mode 100755 index 0000000000000000000000000000000000000000..ccbac4c07af02abae2c1bbc93189db82c5519feb --- /dev/null +++ b/src/g_calib/project_points.m @@ -0,0 +1,282 @@ +function [xp,dxpdom,dxpdT,dxpdf,dxpdc,dxpdk] = project_points(X,om,T,f,c,k) + +%project_points.m +% +%[xp,dxpdom,dxpdT,dxpdf,dxpdc,dxpdk] = project_points(X,om,T,f,c,k) +% +%Projects a 3D structure onto the image plane. +% +%INPUT: X: 3D structure in the world coordinate frame (3xN matrix for N points) +% (om,T): Rigid motion parameters between world coordinate frame and camera reference frame +% om: rotation vector (3x1 vector); T: translation vector (3x1 vector) +% f: camera focal length in units of horizontal and vertical pixel units (2x1 vector) +% c: principal point location in pixel units (2x1 vector) +% k: Distortion coefficients (radial and tangential) (4x1 vector) +% +%OUTPUT: xp: Projected pixel coordinates (2xN matrix for N points) +% dxpdom: Derivative of xp with respect to om ((2N)x3 matrix) +% dxpdT: Derivative of xp with respect to T ((2N)x3 matrix) +% dxpdf: Derivative of xp with respect to f ((2N)x2 matrix if f is 2x1, or (2N)x1 matrix is f is a scalar) +% dxpdc: Derivative of xp with respect to c ((2N)x2 matrix) +% dxpdk: Derivative of xp with respect to k ((2N)x4 matrix) +% +%Definitions: +%Let P be a point in 3D of coordinates X in the world reference frame (stored in the matrix X) +%The coordinate vector of P in the camera reference frame is: Xc = R*X + T +%where R is the rotation matrix corresponding to the rotation vector om: R = rodrigues(om); +%call x, y and z the 3 coordinates of Xc: x = Xc(1); y = Xc(2); z = Xc(3); +%The pinehole projection coordinates of P is [a;b] where a=x/z and b=y/z. +%call r^2 = a^2 + b^2. +%The distorted point coordinates are: xd = [xx;yy] where: +% +%xx = a * (1 + kc(1)*r^2 + kc(2)*r^4) + 2*kc(3)*a*b + kc(4)*(r^2 + 2*a^2); +%yy = b * (1 + kc(1)*r^2 + kc(2)*r^4) + kc(3)*(r^2 + 2*b^2) + 2*kc(4)*a*b; +% +%The left terms correspond to radial distortion, the right terms correspond to tangential distortion +% +%Fianlly, convertion into pixel coordinates: The final pixel coordinates vector xp=[xxp;yyp] where: +% +%xxp = f(1)*xx + c(1) +%yyp = f(2)*yy + c(2) +% +% +%NOTE: About 90 percent of the code takes care fo computing the Jacobian matrices +% +% +%Important function called within that program: +% +%rodrigues.m: Computes the rotation matrix corresponding to a rotation vector +% +%rigid_motion.m: Computes the rigid motion transformation of a given structure + + + +if nargin < 6, + k = zeros(4,1); + if nargin < 5, + c = zeros(2,1); + if nargin < 4, + f = ones(2,1); + if nargin < 3, + T = zeros(3,1); + if nargin < 2, + om = zeros(3,1); + if nargin < 1, + error('Need at least a 3D structure to project (in project_points.m)'); + return; + end; + end; + end; + end; + end; +end; + + +[m,n] = size(X); + +[Y,dYdom,dYdT] = rigid_motion(X,om,T); + + +inv_Z = 1./Y(3,:); + +x = (Y(1:2,:) .* (ones(2,1) * inv_Z)) ; + + +bb = (-x(1,:) .* inv_Z)'*ones(1,3); +cc = (-x(2,:) .* inv_Z)'*ones(1,3); + + +dxdom = zeros(2*n,3); +dxdom(1:2:end,:) = ((inv_Z')*ones(1,3)) .* dYdom(1:3:end,:) + bb .* dYdom(3:3:end,:); +dxdom(2:2:end,:) = ((inv_Z')*ones(1,3)) .* dYdom(2:3:end,:) + cc .* dYdom(3:3:end,:); + +dxdT = zeros(2*n,3); +dxdT(1:2:end,:) = ((inv_Z')*ones(1,3)) .* dYdT(1:3:end,:) + bb .* dYdT(3:3:end,:); +dxdT(2:2:end,:) = ((inv_Z')*ones(1,3)) .* dYdT(2:3:end,:) + cc .* dYdT(3:3:end,:); + + +% Add distortion: + +r2 = x(1,:).^2 + x(2,:).^2; + + + +dr2dom = 2*((x(1,:)')*ones(1,3)) .* dxdom(1:2:end,:) + 2*((x(2,:)')*ones(1,3)) .* dxdom(2:2:end,:); +dr2dT = 2*((x(1,:)')*ones(1,3)) .* dxdT(1:2:end,:) + 2*((x(2,:)')*ones(1,3)) .* dxdT(2:2:end,:); + + +r4 = r2.^2; + +dr4dom = 2*((r2')*ones(1,3)) .* dr2dom; +dr4dT = 2*((r2')*ones(1,3)) .* dr2dT; + + +% Radial distortion: + +cdist = 1 + k(1) * r2 + k(2) * r4; + +dcdistdom = k(1) * dr2dom + k(2) * dr4dom; +dcdistdT = k(1) * dr2dT+ k(2) * dr4dT; +dcdistdk = [ r2' r4' zeros(n,2)]; + + +xd1 = x .* (ones(2,1)*cdist); + +dxd1dom = zeros(2*n,3); +dxd1dom(1:2:end,:) = (x(1,:)'*ones(1,3)) .* dcdistdom; +dxd1dom(2:2:end,:) = (x(2,:)'*ones(1,3)) .* dcdistdom; +coeff = (reshape([cdist;cdist],2*n,1)*ones(1,3)); +dxd1dom = dxd1dom + coeff.* dxdom; + +dxd1dT = zeros(2*n,3); +dxd1dT(1:2:end,:) = (x(1,:)'*ones(1,3)) .* dcdistdT; +dxd1dT(2:2:end,:) = (x(2,:)'*ones(1,3)) .* dcdistdT; +dxd1dT = dxd1dT + coeff.* dxdT; + +dxd1dk = zeros(2*n,4); +dxd1dk(1:2:end,:) = (x(1,:)'*ones(1,4)) .* dcdistdk; +dxd1dk(2:2:end,:) = (x(2,:)'*ones(1,4)) .* dcdistdk; + + + +% tangential distortion: + +a1 = 2.*x(1,:).*x(2,:); +a2 = r2 + 2*x(1,:).^2; +a3 = r2 + 2*x(2,:).^2; + +delta_x = [k(3)*a1 + k(4)*a2 ; + k(3) * a3 + k(4)*a1]; + + +ddelta_xdx = zeros(2*n,2*n); +aa = (2*k(3)*x(2,:)+6*k(4)*x(1,:))'*ones(1,3); +bb = (2*k(3)*x(1,:)+2*k(4)*x(2,:))'*ones(1,3); +cc = (6*k(3)*x(2,:)+2*k(4)*x(1,:))'*ones(1,3); + +ddelta_xdom = zeros(2*n,3); +ddelta_xdom(1:2:end,:) = aa .* dxdom(1:2:end,:) + bb .* dxdom(2:2:end,:); +ddelta_xdom(2:2:end,:) = bb .* dxdom(1:2:end,:) + cc .* dxdom(2:2:end,:); + +ddelta_xdT = zeros(2*n,3); +ddelta_xdT(1:2:end,:) = aa .* dxdT(1:2:end,:) + bb .* dxdT(2:2:end,:); +ddelta_xdT(2:2:end,:) = bb .* dxdT(1:2:end,:) + cc .* dxdT(2:2:end,:); + +ddelta_xdk = zeros(2*n,4); +ddelta_xdk(1:2:end,3) = a1'; +ddelta_xdk(1:2:end,4) = a2'; +ddelta_xdk(2:2:end,3) = a3'; +ddelta_xdk(2:2:end,4) = a1'; + + + +xd2 = xd1 + delta_x; + +dxd2dom = dxd1dom + ddelta_xdom ; +dxd2dT = dxd1dT + ddelta_xdT; +dxd2dk = dxd1dk + ddelta_xdk ; + + +% Pixel coordinates: +if length(f)>1, + xp = xd2 .* (f * ones(1,n)) + c*ones(1,n); + coeff = reshape(f*ones(1,n),2*n,1); + dxpdom = (coeff*ones(1,3)) .* dxd2dom; + dxpdT = (coeff*ones(1,3)) .* dxd2dT; + dxpdk = (coeff*ones(1,5)) .* dxd2dk; + dxpdf = zeros(2*n,2); + dxpdf(1:2:end,1) = xd2(1,:)'; + dxpdf(2:2:end,2) = xd2(2,:)'; +else + xp = f * xd2 + c*ones(1,n); + dxpdom = f * dxd2dom; + dxpdT = f * dxd2dT; + dxpdk = f * dxd2dk; + dxpdf = xd2(:); +end; + +dxpdc = zeros(2*n,2); +dxpdc(1:2:end,1) = ones(n,1); +dxpdc(2:2:end,2) = ones(n,1); + + + + +return; + +% Test of the Jacobians: + +n = 10; + +X = 10*randn(3,n); +om = randn(3,1); +T = [10*randn(2,1);40]; +f = 1000*rand(2,1); +c = 1000*randn(2,1); +k = 0.5*randn(4,1); + + +[x,dxdom,dxdT,dxdf,dxdc,dxdk] = project_points(X,om,T,f,c,k); + + +% Test on om: OK + +dom = 0.000000001 * norm(om)*randn(3,1); +om2 = om + dom; + +[x2] = project_points(X,om2,T,f,c,k); + +x_pred = x + reshape(dxdom * dom,2,n); + + +norm(x2-x)/norm(x2 - x_pred) + + +% Test on T: OK!! + +dT = 0.0001 * norm(T)*randn(3,1); +T2 = T + dT; + +[x2] = project_points(X,om,T2,f,c,k); + +x_pred = x + reshape(dxdT * dT,2,n); + + +norm(x2-x)/norm(x2 - x_pred) + + + +% Test on f: OK!! + +df = 0.001 * norm(f)*randn(2,1); +f2 = f + df; + +[x2] = project_points(X,om,T,f2,c,k); + +x_pred = x + reshape(dxdf * df,2,n); + + +norm(x2-x)/norm(x2 - x_pred) + + +% Test on c: OK!! + +dc = 0.01 * norm(c)*randn(2,1); +c2 = c + dc; + +[x2] = project_points(X,om,T,f,c2,k); + +x_pred = x + reshape(dxdc * dc,2,n); + +norm(x2-x)/norm(x2 - x_pred) + +% Test on k: OK!! + +dk = 0.001 * norm(4)*randn(4,1); +k2 = k + dk; + +[x2] = project_points(X,om,T,f,c,k2); + +x_pred = x + reshape(dxdk * dk,2,n); + +norm(x2-x)/norm(x2 - x_pred) diff --git a/src/g_calib/project_points2.m b/src/g_calib/project_points2.m new file mode 100755 index 0000000000000000000000000000000000000000..24918ec063b5dda7c822671919fcbc5c4186fdbc --- /dev/null +++ b/src/g_calib/project_points2.m @@ -0,0 +1,342 @@ +function [xp,dxpdom,dxpdT,dxpdf,dxpdc,dxpdk,dxpdalpha] = project_points2(X,om,T,f,c,k,alpha) + +%project_points2.m +% +%[xp,dxpdom,dxpdT,dxpdf,dxpdc,dxpdk] = project_points2(X,om,T,f,c,k,alpha) +% +%Projects a 3D structure onto the image plane. +% +%INPUT: X: 3D structure in the world coordinate frame (3xN matrix for N points) +% (om,T): Rigid motion parameters between world coordinate frame and camera reference frame +% om: rotation vector (3x1 vector); T: translation vector (3x1 vector) +% f: camera focal length in units of horizontal and vertical pixel units (2x1 vector) +% c: principal point location in pixel units (2x1 vector) +% k: Distortion coefficients (radial and tangential) (4x1 vector) +% alpha: Skew coefficient between x and y pixel (alpha = 0 <=> square pixels) +% +%OUTPUT: xp: Projected pixel coordinates (2xN matrix for N points) +% dxpdom: Derivative of xp with respect to om ((2N)x3 matrix) +% dxpdT: Derivative of xp with respect to T ((2N)x3 matrix) +% dxpdf: Derivative of xp with respect to f ((2N)x2 matrix if f is 2x1, or (2N)x1 matrix is f is a scalar) +% dxpdc: Derivative of xp with respect to c ((2N)x2 matrix) +% dxpdk: Derivative of xp with respect to k ((2N)x4 matrix) +% +%Definitions: +%Let P be a point in 3D of coordinates X in the world reference frame (stored in the matrix X) +%The coordinate vector of P in the camera reference frame is: Xc = R*X + T +%where R is the rotation matrix corresponding to the rotation vector om: R = rodrigues(om); +%call x, y and z the 3 coordinates of Xc: x = Xc(1); y = Xc(2); z = Xc(3); +%The pinehole projection coordinates of P is [a;b] where a=x/z and b=y/z. +%call r^2 = a^2 + b^2. +%The distorted point coordinates are: xd = [xx;yy] where: +% +%xx = a * (1 + kc(1)*r^2 + kc(2)*r^4 + kc(5)*r^6) + 2*kc(3)*a*b + kc(4)*(r^2 + 2*a^2); +%yy = b * (1 + kc(1)*r^2 + kc(2)*r^4 + kc(5)*r^6) + kc(3)*(r^2 + 2*b^2) + 2*kc(4)*a*b; +% +%The left terms correspond to radial distortion (6th degree), the right terms correspond to tangential distortion +% +%Finally, convertion into pixel coordinates: The final pixel coordinates vector xp=[xxp;yyp] where: +% +%xxp = f(1)*(xx + alpha*yy) + c(1) +%yyp = f(2)*yy + c(2) +% +% +%NOTE: About 90 percent of the code takes care fo computing the Jacobian matrices +% +% +%Important function called within that program: +% +%rodrigues.m: Computes the rotation matrix corresponding to a rotation vector +% +%rigid_motion.m: Computes the rigid motion transformation of a given structure + + +if nargin < 7, + alpha = 0; + if nargin < 6, + k = zeros(5,1); + if nargin < 5, + c = zeros(2,1); + if nargin < 4, + f = ones(2,1); + if nargin < 3, + T = zeros(3,1); + if nargin < 2, + om = zeros(3,1); + if nargin < 1, + error('Need at least a 3D structure to project (in project_points.m)'); + return; + end; + end; + end; + end; + end; + end; +end; + + +[m,n] = size(X); + +if nargout > 1, + [Y,dYdom,dYdT] = rigid_motion(X,om,T); +else + Y = rigid_motion(X,om,T); +end; + + +inv_Z = 1./Y(3,:); + +x = (Y(1:2,:) .* (ones(2,1) * inv_Z)) ; + + +bb = (-x(1,:) .* inv_Z)'*ones(1,3); +cc = (-x(2,:) .* inv_Z)'*ones(1,3); + +if nargout > 1, + dxdom = zeros(2*n,3); + dxdom(1:2:end,:) = ((inv_Z')*ones(1,3)) .* dYdom(1:3:end,:) + bb .* dYdom(3:3:end,:); + dxdom(2:2:end,:) = ((inv_Z')*ones(1,3)) .* dYdom(2:3:end,:) + cc .* dYdom(3:3:end,:); + + dxdT = zeros(2*n,3); + dxdT(1:2:end,:) = ((inv_Z')*ones(1,3)) .* dYdT(1:3:end,:) + bb .* dYdT(3:3:end,:); + dxdT(2:2:end,:) = ((inv_Z')*ones(1,3)) .* dYdT(2:3:end,:) + cc .* dYdT(3:3:end,:); +end; + + +% Add distortion: + +r2 = x(1,:).^2 + x(2,:).^2; + +if nargout > 1, + dr2dom = 2*((x(1,:)')*ones(1,3)) .* dxdom(1:2:end,:) + 2*((x(2,:)')*ones(1,3)) .* dxdom(2:2:end,:); + dr2dT = 2*((x(1,:)')*ones(1,3)) .* dxdT(1:2:end,:) + 2*((x(2,:)')*ones(1,3)) .* dxdT(2:2:end,:); +end; + + +r4 = r2.^2; + +if nargout > 1, + dr4dom = 2*((r2')*ones(1,3)) .* dr2dom; + dr4dT = 2*((r2')*ones(1,3)) .* dr2dT; +end + +r6 = r2.^3; + +if nargout > 1, + dr6dom = 3*((r2'.^2)*ones(1,3)) .* dr2dom; + dr6dT = 3*((r2'.^2)*ones(1,3)) .* dr2dT; +end; + +% Radial distortion: + +cdist = 1 + k(1) * r2 + k(2) * r4 + k(5) * r6; + +if nargout > 1, + dcdistdom = k(1) * dr2dom + k(2) * dr4dom + k(5) * dr6dom; + dcdistdT = k(1) * dr2dT + k(2) * dr4dT + k(5) * dr6dT; + dcdistdk = [ r2' r4' zeros(n,2) r6']; +end; + +xd1 = x .* (ones(2,1)*cdist); + +if nargout > 1, + dxd1dom = zeros(2*n,3); + dxd1dom(1:2:end,:) = (x(1,:)'*ones(1,3)) .* dcdistdom; + dxd1dom(2:2:end,:) = (x(2,:)'*ones(1,3)) .* dcdistdom; + coeff = (reshape([cdist;cdist],2*n,1)*ones(1,3)); + dxd1dom = dxd1dom + coeff.* dxdom; + + dxd1dT = zeros(2*n,3); + dxd1dT(1:2:end,:) = (x(1,:)'*ones(1,3)) .* dcdistdT; + dxd1dT(2:2:end,:) = (x(2,:)'*ones(1,3)) .* dcdistdT; + dxd1dT = dxd1dT + coeff.* dxdT; + + dxd1dk = zeros(2*n,5); + dxd1dk(1:2:end,:) = (x(1,:)'*ones(1,5)) .* dcdistdk; + dxd1dk(2:2:end,:) = (x(2,:)'*ones(1,5)) .* dcdistdk; +end; + + +% tangential distortion: + +a1 = 2.*x(1,:).*x(2,:); +a2 = r2 + 2*x(1,:).^2; +a3 = r2 + 2*x(2,:).^2; + +delta_x = [k(3)*a1 + k(4)*a2 ; + k(3) * a3 + k(4)*a1]; + + +%ddelta_xdx = zeros(2*n,2*n); +aa = (2*k(3)*x(2,:)+6*k(4)*x(1,:))'*ones(1,3); +bb = (2*k(3)*x(1,:)+2*k(4)*x(2,:))'*ones(1,3); +cc = (6*k(3)*x(2,:)+2*k(4)*x(1,:))'*ones(1,3); + +if nargout > 1, + ddelta_xdom = zeros(2*n,3); + ddelta_xdom(1:2:end,:) = aa .* dxdom(1:2:end,:) + bb .* dxdom(2:2:end,:); + ddelta_xdom(2:2:end,:) = bb .* dxdom(1:2:end,:) + cc .* dxdom(2:2:end,:); + + ddelta_xdT = zeros(2*n,3); + ddelta_xdT(1:2:end,:) = aa .* dxdT(1:2:end,:) + bb .* dxdT(2:2:end,:); + ddelta_xdT(2:2:end,:) = bb .* dxdT(1:2:end,:) + cc .* dxdT(2:2:end,:); + + ddelta_xdk = zeros(2*n,5); + ddelta_xdk(1:2:end,3) = a1'; + ddelta_xdk(1:2:end,4) = a2'; + ddelta_xdk(2:2:end,3) = a3'; + ddelta_xdk(2:2:end,4) = a1'; +end; + + +xd2 = xd1 + delta_x; + +if nargout > 1, + dxd2dom = dxd1dom + ddelta_xdom ; + dxd2dT = dxd1dT + ddelta_xdT; + dxd2dk = dxd1dk + ddelta_xdk ; +end; + + +% Add Skew: + +xd3 = [xd2(1,:) + alpha*xd2(2,:);xd2(2,:)]; + +% Compute: dxd3dom, dxd3dT, dxd3dk, dxd3dalpha +if nargout > 1, + dxd3dom = zeros(2*n,3); + dxd3dom(1:2:2*n,:) = dxd2dom(1:2:2*n,:) + alpha*dxd2dom(2:2:2*n,:); + dxd3dom(2:2:2*n,:) = dxd2dom(2:2:2*n,:); + dxd3dT = zeros(2*n,3); + dxd3dT(1:2:2*n,:) = dxd2dT(1:2:2*n,:) + alpha*dxd2dT(2:2:2*n,:); + dxd3dT(2:2:2*n,:) = dxd2dT(2:2:2*n,:); + dxd3dk = zeros(2*n,5); + dxd3dk(1:2:2*n,:) = dxd2dk(1:2:2*n,:) + alpha*dxd2dk(2:2:2*n,:); + dxd3dk(2:2:2*n,:) = dxd2dk(2:2:2*n,:); + dxd3dalpha = zeros(2*n,1); + dxd3dalpha(1:2:2*n,:) = xd2(2,:)'; +end; + + + +% Pixel coordinates: +if length(f)>1, + xp = xd3 .* (f(:) * ones(1,n)) + c(:)*ones(1,n); + if nargout > 1, + coeff = reshape(f(:)*ones(1,n),2*n,1); + dxpdom = (coeff*ones(1,3)) .* dxd3dom; + dxpdT = (coeff*ones(1,3)) .* dxd3dT; + dxpdk = (coeff*ones(1,5)) .* dxd3dk; + dxpdalpha = (coeff) .* dxd3dalpha; + dxpdf = zeros(2*n,2); + dxpdf(1:2:end,1) = xd3(1,:)'; + dxpdf(2:2:end,2) = xd3(2,:)'; + end; +else + xp = f * xd3 + c*ones(1,n); + if nargout > 1, + dxpdom = f * dxd3dom; + dxpdT = f * dxd3dT; + dxpdk = f * dxd3dk; + dxpdalpha = f .* dxd3dalpha; + dxpdf = xd3(:); + end; +end; + +if nargout > 1, + dxpdc = zeros(2*n,2); + dxpdc(1:2:end,1) = ones(n,1); + dxpdc(2:2:end,2) = ones(n,1); +end; + + +return; + +% Test of the Jacobians: + +n = 10; + +X = 10*randn(3,n); +om = randn(3,1); +T = [10*randn(2,1);40]; +f = 1000*rand(2,1); +c = 1000*randn(2,1); +k = 0.5*randn(5,1); +alpha = 0.01*randn(1,1); + +[x,dxdom,dxdT,dxdf,dxdc,dxdk,dxdalpha] = project_points2(X,om,T,f,c,k,alpha); + + +% Test on om: OK + +dom = 0.000000001 * norm(om)*randn(3,1); +om2 = om + dom; + +[x2] = project_points2(X,om2,T,f,c,k,alpha); + +x_pred = x + reshape(dxdom * dom,2,n); + + +norm(x2-x)/norm(x2 - x_pred) + + +% Test on T: OK!! + +dT = 0.0001 * norm(T)*randn(3,1); +T2 = T + dT; + +[x2] = project_points2(X,om,T2,f,c,k,alpha); + +x_pred = x + reshape(dxdT * dT,2,n); + + +norm(x2-x)/norm(x2 - x_pred) + + + +% Test on f: OK!! + +df = 0.001 * norm(f)*randn(2,1); +f2 = f + df; + +[x2] = project_points2(X,om,T,f2,c,k,alpha); + +x_pred = x + reshape(dxdf * df,2,n); + + +norm(x2-x)/norm(x2 - x_pred) + + +% Test on c: OK!! + +dc = 0.01 * norm(c)*randn(2,1); +c2 = c + dc; + +[x2] = project_points2(X,om,T,f,c2,k,alpha); + +x_pred = x + reshape(dxdc * dc,2,n); + +norm(x2-x)/norm(x2 - x_pred) + +% Test on k: OK!! + +dk = 0.001 * norm(k)*randn(5,1); +k2 = k + dk; + +[x2] = project_points2(X,om,T,f,c,k2,alpha); + +x_pred = x + reshape(dxdk * dk,2,n); + +norm(x2-x)/norm(x2 - x_pred) + + +% Test on alpha: OK!! + +dalpha = 0.001 * norm(k)*randn(1,1); +alpha2 = alpha + dalpha; + +[x2] = project_points2(X,om,T,f,c,k,alpha2); + +x_pred = x + reshape(dxdalpha * dalpha,2,n); + +norm(x2-x)/norm(x2 - x_pred) diff --git a/src/g_calib/project_points3.m b/src/g_calib/project_points3.m new file mode 100755 index 0000000000000000000000000000000000000000..e44742bea4daef52532c89dcc75398cba6176952 --- /dev/null +++ b/src/g_calib/project_points3.m @@ -0,0 +1,375 @@ +function [xp,dxpdX,dxpdom,dxpdT,dxpdf,dxpdc,dxpdk,dxpdalpha] = project_points3(X,om,T,f,c,k,alpha) + +%[xp,dxpdX,dxpdom,dxpdT,dxpdf,dxpdc,dxpdk] = project_points3(X,om,T,f,c,k,alpha) +% +%Projects a 3D structure onto the image plane. +%Same as project_points2, with the derivative with respect to the shape X (dxpdX) included. +%This matrix is the most hideous thing to compute. +% +%INPUT: X: 3D structure in the world coordinate frame (3xN matrix for N points) +% (om,T): Rigid motion parameters between world coordinate frame and camera reference frame +% om: rotation vector (3x1 vector); T: translation vector (3x1 vector) +% f: camera focal length in units of horizontal and vertical pixel units (2x1 vector) +% c: principal point location in pixel units (2x1 vector) +% k: Distortion coefficients (radial and tangential) (4x1 vector) +% alpha: Skew coefficient between x and y pixel (alpha = 0 <=> square pixels) +% +%OUTPUT: xp: Projected pixel coordinates (2xN matrix for N points) +% dxpdX: Derivative of xp with respect to X (sparse (2N)x(3N) matrix) +% dxpdom: Derivative of xp with respect to om ((2N)x3 matrix) +% dxpdT: Derivative of xp with respect to T ((2N)x3 matrix) +% dxpdf: Derivative of xp with respect to f ((2N)x2 matrix if f is 2x1, or (2N)x1 matrix is f is a scalar) +% dxpdc: Derivative of xp with respect to c ((2N)x2 matrix) +% dxpdk: Derivative of xp with respect to k ((2N)x4 matrix) +% +%Definitions: +%Let P be a point in 3D of coordinates X in the world reference frame (stored in the matrix X) +%The coordinate vector of P in the camera reference frame is: Xc = R*X + T +%where R is the rotation matrix corresponding to the rotation vector om: R = rodrigues(om); +%call x, y and z the 3 coordinates of Xc: x = Xc(1); y = Xc(2); z = Xc(3); +%The pinehole projection coordinates of P is [a;b] where a=x/z and b=y/z. +%call r^2 = a^2 + b^2. +%The distorted point coordinates are: xd = [xx;yy] where: +% +%xx = a * (1 + kc(1)*r^2 + kc(2)*r^4 + kc(5)*r^6) + 2*kc(3)*a*b + kc(4)*(r^2 + 2*a^2); +%yy = b * (1 + kc(1)*r^2 + kc(2)*r^4 + kc(5)*r^6) + kc(3)*(r^2 + 2*b^2) + 2*kc(4)*a*b; +% +%The left terms correspond to radial distortion (6th degree), the right terms correspond to tangential distortion +% +%Finally, convertion into pixel coordinates: The final pixel coordinates vector xp=[xxp;yyp] where: +% +%xxp = f(1)*(xx + alpha*yy) + c(1) +%yyp = f(2)*yy + c(2) +% +% +%NOTE: About 90 percent of the code takes care fo computing the Jacobian matrices +% +% +%Important function called within that program: +% +%rodrigues.m: Computes the rotation matrix corresponding to a rotation vector +% +%rigid_motion2.m: Computes the rigid motion transformation of a given structure + + +if nargin < 7, + alpha = 0; + if nargin < 6, + k = zeros(5,1); + if nargin < 5, + c = zeros(2,1); + if nargin < 4, + f = ones(2,1); + if nargin < 3, + T = zeros(3,1); + if nargin < 2, + om = zeros(3,1); + if nargin < 1, + error('Need at least a 3D structure to project (in project_points.m)'); + return; + end; + end; + end; + end; + end; + end; +end; + + +[m,n] = size(X); + + +% Rigid motion: + +[Y,dYdom,dYdT] = rigid_motion(X,om,T); + +% The derivative of Y with respect to X -> just a series of R matrices (needed for later) +R = rodrigues(om); +dYdX = repmat(R,1,n); %[reshape(R(1,:)'*ones(1,n),3*n,1)';reshape(R(2,:)'*ones(1,n),3*n,1)';reshape(R(3,:)'*ones(1,n),3*n,1)']; + +%keyboard; + +% Pinehole projection: + +inv_Z = 1./Y(3,:); + +x = (Y(1:2,:) .* (ones(2,1) * inv_Z)) ; + +bb = (-x(1,:) .* inv_Z)'*ones(1,3); +cc = (-x(2,:) .* inv_Z)'*ones(1,3); + + +dxdom = zeros(2*n,3); +dxdom(1:2:end,:) = ((inv_Z')*ones(1,3)) .* dYdom(1:3:end,:) + bb .* dYdom(3:3:end,:); +dxdom(2:2:end,:) = ((inv_Z')*ones(1,3)) .* dYdom(2:3:end,:) + cc .* dYdom(3:3:end,:); + +dxdT = zeros(2*n,3); +dxdT(1:2:end,:) = ((inv_Z')*ones(1,3)) .* dYdT(1:3:end,:) + bb .* dYdT(3:3:end,:); +dxdT(2:2:end,:) = ((inv_Z')*ones(1,3)) .* dYdT(2:3:end,:) + cc .* dYdT(3:3:end,:); + +AA_temp = (ones(3,1)*reshape(ones(3,1)*inv_Z,n*3,1)') .* dYdX; % product of R and inv_Z + +dxdX = [AA_temp(1,:) - (reshape(ones(3,1)*x(1,:),3*n,1)'.* AA_temp(3,:)); +AA_temp(2,:) - (reshape(ones(3,1)*x(2,:),3*n,1)'.* AA_temp(3,:))]; + + +% Add distortion: + +r2 = x(1,:).^2 + x(2,:).^2; + +dr2dom = 2*((x(1,:)')*ones(1,3)) .* dxdom(1:2:end,:) + 2*((x(2,:)')*ones(1,3)) .* dxdom(2:2:end,:); +dr2dT = 2*((x(1,:)')*ones(1,3)) .* dxdT(1:2:end,:) + 2*((x(2,:)')*ones(1,3)) .* dxdT(2:2:end,:); + +dr2dX = sum(2*([reshape(ones(3,1)*x(1,:),3*n,1)';reshape(ones(3,1)*x(2,:),3*n,1)']).*dxdX); + + +r4 = r2.^2; + +dr4dom = 2*((r2')*ones(1,3)) .* dr2dom; +dr4dT = 2*((r2')*ones(1,3)) .* dr2dT; + +dr4dX = 2*reshape(ones(3,1)*r2,3*n,1)' .* dr2dX; + + +r6 = r2.^3; + +dr6dom = 3*(r4'*ones(1,3)) .* dr2dom; +dr6dT = 3*(r4'*ones(1,3)) .* dr2dT; +dr6dX = 2*reshape(ones(3,1)*r4,3*n,1)' .* dr2dX; + + +% Radial distortion: + +cdist = 1 + k(1) * r2 + k(2) * r4 + k(5) * r6; + +dcdistdom = k(1) * dr2dom + k(2) * dr4dom + k(5) * dr6dom; +dcdistdT = k(1) * dr2dT + k(2) * dr4dT + k(5) * dr6dT; +dcdistdk = [ r2' r4' zeros(n,2) r6']; +dcdistdX = k(1) * dr2dX + k(2) * dr4dX + k(5) * dr6dX; + + +xd1 = x .* (ones(2,1)*cdist); + +dxd1dom = zeros(2*n,3); +dxd1dom(1:2:end,:) = (x(1,:)'*ones(1,3)) .* dcdistdom; +dxd1dom(2:2:end,:) = (x(2,:)'*ones(1,3)) .* dcdistdom; +coeff = (reshape([cdist;cdist],2*n,1)*ones(1,3)); +dxd1dom = dxd1dom + coeff.* dxdom; + +dxd1dT = zeros(2*n,3); +dxd1dT(1:2:end,:) = (x(1,:)'*ones(1,3)) .* dcdistdT; +dxd1dT(2:2:end,:) = (x(2,:)'*ones(1,3)) .* dcdistdT; +dxd1dT = dxd1dT + coeff.* dxdT; + +dxd1dk = zeros(2*n,5); +dxd1dk(1:2:end,:) = (x(1,:)'*ones(1,5)) .* dcdistdk; +dxd1dk(2:2:end,:) = (x(2,:)'*ones(1,5)) .* dcdistdk; + +dxd1dX = [dcdistdX .* (reshape(ones(3,1)*x(1,:),3*n,1)');dcdistdX .* (reshape(ones(3,1)*x(2,:),3*n,1)')] + (ones(2,1)*reshape(ones(3,1)*cdist,3*n,1)').*dxdX; + + + +% tangential distortion: + +a1 = 2.*x(1,:).*x(2,:); +a2 = r2 + 2*x(1,:).^2; +a3 = r2 + 2*x(2,:).^2; + +delta_x = [k(3)*a1 + k(4)*a2 ; + k(3) * a3 + k(4)*a1]; + + +%ddelta_xdx = zeros(2*n,2*n); +aa = (2*k(3)*x(2,:)+6*k(4)*x(1,:))'*ones(1,3); +bb = (2*k(3)*x(1,:)+2*k(4)*x(2,:))'*ones(1,3); +cc = (6*k(3)*x(2,:)+2*k(4)*x(1,:))'*ones(1,3); + +ddelta_xdom = zeros(2*n,3); +ddelta_xdom(1:2:end,:) = aa .* dxdom(1:2:end,:) + bb .* dxdom(2:2:end,:); +ddelta_xdom(2:2:end,:) = bb .* dxdom(1:2:end,:) + cc .* dxdom(2:2:end,:); + +ddelta_xdT = zeros(2*n,3); +ddelta_xdT(1:2:end,:) = aa .* dxdT(1:2:end,:) + bb .* dxdT(2:2:end,:); +ddelta_xdT(2:2:end,:) = bb .* dxdT(1:2:end,:) + cc .* dxdT(2:2:end,:); + +ddelta_xdk = zeros(2*n,5); +ddelta_xdk(1:2:end,3) = a1'; +ddelta_xdk(1:2:end,4) = a2'; +ddelta_xdk(2:2:end,3) = a3'; +ddelta_xdk(2:2:end,4) = a1'; + + +ddelta_xdX = [2*k(3)*(dxdX(1,:) .* (reshape(ones(3,1)*x(2,:),3*n,1)') + dxdX(2,:) .* (reshape(ones(3,1)*x(1,:),3*n,1)')) + ... + k(4) * (6 * (dxdX(1,:) .* (reshape(ones(3,1)*x(1,:),3*n,1)')) + 2 * (dxdX(2,:) .* (reshape(ones(3,1)*x(2,:),3*n,1)'))) ; +k(3) * (6* (dxdX(2,:) .* (reshape(ones(3,1)*x(2,:),3*n,1)')) + 2*(dxdX(1,:) .* (reshape(ones(3,1)*x(1,:),3*n,1)'))) + ... + 2* k(4)* (dxdX(1,:) .* (reshape(ones(3,1)*x(2,:),3*n,1)') + dxdX(2,:) .* (reshape(ones(3,1)*x(1,:),3*n,1)'))]; + +xd2 = xd1 + delta_x; + +dxd2dom = dxd1dom + ddelta_xdom ; +dxd2dT = dxd1dT + ddelta_xdT; +dxd2dk = dxd1dk + ddelta_xdk ; +dxd2dX = dxd1dX + ddelta_xdX ; + + +% Add Skew: + +xd3 = [xd2(1,:) + alpha*xd2(2,:);xd2(2,:)]; + +% Compute: dxd3dom, dxd3dT, dxd3dk, dxd3dalpha + +dxd3dom = zeros(2*n,3); +dxd3dom(1:2:2*n,:) = dxd2dom(1:2:2*n,:) + alpha*dxd2dom(2:2:2*n,:); +dxd3dom(2:2:2*n,:) = dxd2dom(2:2:2*n,:); +dxd3dT = zeros(2*n,3); +dxd3dT(1:2:2*n,:) = dxd2dT(1:2:2*n,:) + alpha*dxd2dT(2:2:2*n,:); +dxd3dT(2:2:2*n,:) = dxd2dT(2:2:2*n,:); +dxd3dk = zeros(2*n,5); +dxd3dk(1:2:2*n,:) = dxd2dk(1:2:2*n,:) + alpha*dxd2dk(2:2:2*n,:); +dxd3dk(2:2:2*n,:) = dxd2dk(2:2:2*n,:); +dxd3dalpha = zeros(2*n,1); +dxd3dalpha(1:2:2*n,:) = xd2(2,:)'; + + +dxd3dX = [dxd2dX(1,:) + alpha*dxd2dX(2,:); dxd2dX(2,:)]; + + + +% Pixel coordinates: +if length(f)>1, + xp = xd3 .* (f * ones(1,n)) + c*ones(1,n); + coeff = reshape(f*ones(1,n),2*n,1); + dxpdom = (coeff*ones(1,3)) .* dxd3dom; + dxpdT = (coeff*ones(1,3)) .* dxd3dT; + dxpdk = (coeff*ones(1,5)) .* dxd3dk; + dxpdalpha = (coeff) .* dxd3dalpha; + dxpdf = zeros(2*n,2); + dxpdf(1:2:end,1) = xd3(1,:)'; + dxpdf(2:2:end,2) = xd3(2,:)'; + dxpdX2 = [f(1)*dxd3dX(1,:);f(2)*dxd3dX(2,:)]; +else + xp = f * xd3 + c*ones(1,n); + dxpdom = f * dxd3dom; + dxpdT = f * dxd3dT; + dxpdk = f * dxd3dk; + dxpdalpha = f .* dxd3dalpha; + dxpdf = xd3(:); + dxpdX2 = f * dxd3dX; +end; + +dxpdc = zeros(2*n,2); +dxpdc(1:2:end,1) = ones(n,1); +dxpdc(2:2:end,2) = ones(n,1); + + +% Make a sparse matrix out of the Jacobian matrix dxpdX (to make it valid for matrix +% multiplication): + +II = reshape(reshape([1:2:2*n 2:2:2*n]'*ones(1,3),n,6)',6*n,1); +JJ = reshape(ones(2,1)*[1:3*n],6*n,1); +dxpdX = sparse(II,JJ,dxpdX2(:),2*n,3*n, 6*n); + + +return; + +% Test of the Jacobians: + +n = 10; + +X = 10*randn(3,n) + [zeros(2,n);50*ones(1,n)]; +om = 0.1*randn(3,1); +T = 0.1*(10*randn(3,1) + [0;0;40]); +f = 300 + 200*rand(2,1); +c = 1000*rand(2,1); +k = 0.5*randn(5,1); +alpha = 0.01*randn(1,1); + +[x,dxdX,dxdom,dxdT,dxdf,dxdc,dxdk,dxdalpha] = project_points3(X,om,T,f,c,k,alpha); + + +% Test on X: + +dX = 0.0001 *randn(3,n); +X2 = X + dX; + +[x2] = project_points2(X2,om,T,f,c,k,alpha); + +x_pred = x + reshape(dxdX * dX(:),2,n); + + +norm(x2-x)/norm(x2 - x_pred) + + +% Test on om: + +dom = 0.000000001 * norm(om)*randn(3,1); +om2 = om + dom; + +[x2] = project_points2(X,om2,T,f,c,k,alpha); + +x_pred = x + reshape(dxdom * dom,2,n); + + +norm(x2-x)/norm(x2 - x_pred) + + +% Test on T: OK!! + +dT = 0.0001 * norm(T)*randn(3,1); +T2 = T + dT; + +[x2] = project_points2(X,om,T2,f,c,k,alpha); + +x_pred = x + reshape(dxdT * dT,2,n); + + +norm(x2-x)/norm(x2 - x_pred) + + + +% Test on f: OK!! + +df = 0.00001 * norm(f)*randn(2,1); +f2 = f + df; + +[x2] = project_points2(X,om,T,f2,c,k,alpha); + +x_pred = x + reshape(dxdf * df,2,n); + + +norm(x2-x)/norm(x2 - x_pred) + + +% Test on c: OK!! + +dc = 0.01 * norm(c)*randn(2,1); +c2 = c + dc; + +[x2] = project_points2(X,om,T,f,c2,k,alpha); + +x_pred = x + reshape(dxdc * dc,2,n); + +norm(x2-x)/norm(x2 - x_pred) + +% Test on k: OK!! + +dk = 0.001 * norm(k)*randn(5,1); +k2 = k + dk; + +[x2] = project_points2(X,om,T,f,c,k2,alpha); + +x_pred = x + reshape(dxdk * dk,2,n); + +norm(x2-x)/norm(x2 - x_pred) + + +% Test on alpha: OK!! + +dalpha = 0.001 * norm(k)*randn(1,1); +alpha2 = alpha + dalpha; + +[x2] = project_points2(X,om,T,f,c,k,alpha2); + +x_pred = x + reshape(dxdalpha * dalpha,2,n); + +norm(x2-x)/norm(x2 - x_pred) diff --git a/src/g_calib/project_points_fisheye.m b/src/g_calib/project_points_fisheye.m new file mode 100755 index 0000000000000000000000000000000000000000..8e4548d0c728eb3b9f2ce32a5d59900e1c2bf4d3 --- /dev/null +++ b/src/g_calib/project_points_fisheye.m @@ -0,0 +1,330 @@ +function [xp,dxpdom,dxpdT,dxpdf,dxpdc,dxpdk,dxpdalpha] = project_points_fisheye(X,om,T,f,c,k,alpha) + +%project_points2.m +% +%[xp,dxpdom,dxpdT,dxpdf,dxpdc,dxpdk] = project_points_fisheye(X,om,T,f,c,k,alpha) +% +%Projects a 3D structure onto the image plane of a fisheye camera. +% +%INPUT: X: 3D structure in the world coordinate frame (3xN matrix for N points) +% (om,T): Rigid motion parameters between world coordinate frame and camera reference frame +% om: rotation vector (3x1 vector); T: translation vector (3x1 vector) +% f: camera focal length in units of horizontal and vertical pixel units (2x1 vector) +% c: principal point location in pixel units (2x1 vector) +% k: Distortion fisheye coefficients (5x1 vector) +% alpha: Skew coefficient between x and y pixel (alpha = 0 <=> square pixels) +% +%OUTPUT: xp: Projected pixel coordinates (2xN matrix for N points) +% dxpdom: Derivative of xp with respect to om ((2N)x3 matrix) +% dxpdT: Derivative of xp with respect to T ((2N)x3 matrix) +% dxpdf: Derivative of xp with respect to f ((2N)x2 matrix if f is 2x1, or (2N)x1 matrix is f is a scalar) +% dxpdc: Derivative of xp with respect to c ((2N)x2 matrix) +% dxpdk: Derivative of xp with respect to k ((2N)x5 matrix) +% +%Definitions: +%Let P be a point in 3D of coordinates X in the world reference frame (stored in the matrix X) +%The coordinate vector of P in the camera reference frame is: Xc = R*X + T +%where R is the rotation matrix corresponding to the rotation vector om: R = rodrigues(om); +%call x, y and z the 3 coordinates of Xc: x = Xc(1); y = Xc(2); z = Xc(3); +%The pinehole projection coordinates of P is [a;b] where a=x/z and b=y/z. +%call r^2 = a^2 + b^2, +%call theta = atan(r), +%Fisheye distortion -> theta_d = theta * (1 + k(1)*theta^2 + k(2)*theta^4 + k(3)*theta^6 + k(4)*theta^8) +% +%The distorted point coordinates are: xd = [xx;yy] where: +% +%xx = (theta_d / r) * x +%yy = (theta_d / r) * y +% +%Finally, convertion into pixel coordinates: The final pixel coordinates vector xp=[xxp;yyp] where: +% +%xxp = f(1)*(xx + alpha*yy) + c(1) +%yyp = f(2)*yy + c(2) +% +% +%NOTE: About 90 percent of the code takes care fo computing the Jacobian matrices +% +% +%Important function called within that program: +% +%rodrigues.m: Computes the rotation matrix corresponding to a rotation vector +% +%rigid_motion.m: Computes the rigid motion transformation of a given structure + + +if nargin < 7, + alpha = 0; + if nargin < 6, + k = zeros(4,1); + if nargin < 5, + c = zeros(2,1); + if nargin < 4, + f = ones(2,1); + if nargin < 3, + T = zeros(3,1); + if nargin < 2, + om = zeros(3,1); + if nargin < 1, + error('Need at least a 3D structure to project (in project_points.m)'); + return; + end; + end; + end; + end; + end; + end; +end; + +[m,n] = size(X); + +if nargout > 1, + [Y,dYdom,dYdT] = rigid_motion(X,om,T); +else + Y = rigid_motion(X,om,T); +end; + +inv_Z = 1./Y(3,:); + +x = (Y(1:2,:) .* (ones(2,1) * inv_Z)) ; + +bb = (-x(1,:) .* inv_Z)'*ones(1,3); +cc = (-x(2,:) .* inv_Z)'*ones(1,3); + +if nargout > 1, + dxdom = zeros(2*n,3); + dxdom(1:2:end,:) = ((inv_Z')*ones(1,3)) .* dYdom(1:3:end,:) + bb .* dYdom(3:3:end,:); + dxdom(2:2:end,:) = ((inv_Z')*ones(1,3)) .* dYdom(2:3:end,:) + cc .* dYdom(3:3:end,:); + + dxdT = zeros(2*n,3); + dxdT(1:2:end,:) = ((inv_Z')*ones(1,3)) .* dYdT(1:3:end,:) + bb .* dYdT(3:3:end,:); + dxdT(2:2:end,:) = ((inv_Z')*ones(1,3)) .* dYdT(2:3:end,:) + cc .* dYdT(3:3:end,:); +end; + +% Add fisheye distortion: + +r2 = x(1,:).^2 + x(2,:).^2; + +if nargout > 1, + dr2dom = 2*((x(1,:)')*ones(1,3)) .* dxdom(1:2:end,:) + 2*((x(2,:)')*ones(1,3)) .* dxdom(2:2:end,:); + dr2dT = 2*((x(1,:)')*ones(1,3)) .* dxdT(1:2:end,:) + 2*((x(2,:)')*ones(1,3)) .* dxdT(2:2:end,:); +end; + +% Radial distance: +r = sqrt(r2); +if nargout > 1, + drdr2 = ones(1,length(r)); + drdr2(r>1e-8) = 1 ./ (2*r(r>1e-8)); + + drdom = [ (drdr2').*dr2dom(:,1) (drdr2').*dr2dom(:,2) (drdr2').*dr2dom(:,3) ]; + drdT = [ (drdr2').*dr2dT(:,1) (drdr2').*dr2dT(:,2) (drdr2').*dr2dT(:,3) ]; +end; + +% Angle of the incoming ray: +theta = atan(r); +if nargout > 1, + dthetadr = 1 ./ (1 + r2); + + dthetadom = [ (dthetadr').*drdom(:,1) (dthetadr').*drdom(:,2) (dthetadr').*drdom(:,3) ]; + dthetadT = [ (dthetadr').*drdT(:,1) (dthetadr').*drdT(:,2) (dthetadr').*drdT(:,3) ]; +end; + +% Add the fisheye distortion: + +theta2 = theta.^2; +theta3 = theta2.*theta; +theta4 = theta2.^2; +theta5 = theta4.*theta; +theta6 = theta3.^2; +theta7 = theta6.*theta; +theta8 = theta4.*theta4; +theta9 = theta8.*theta; + +% Fisheye distortion -> theta_d = theta * (1 + k(1)*theta2 + k(2)*theta4 + k(3)*theta6 + k(4)*theta8) + +theta_d = theta + k(1)*theta3 + k(2)*theta5 + k(3)*theta7 + k(4)*theta9; + +if nargout > 1, + dtheta_ddtheta = 1 + 3*k(1)*theta2 + 5*k(2)*theta4 + 7*k(3)*theta6 + 9*k(4)*theta8; + dtheta_ddom = [ (dtheta_ddtheta').*dthetadom(:,1) (dtheta_ddtheta').*dthetadom(:,2) (dtheta_ddtheta').*dthetadom(:,3) ]; + dtheta_ddT = [ (dtheta_ddtheta').*dthetadT(:,1) (dtheta_ddtheta').*dthetadT(:,2) (dtheta_ddtheta').*dthetadT(:,3) ]; + dtheta_ddk = [theta3' theta5' theta7' theta9']; +end; + +% ratio: +inv_r = ones(1,length(r)); +inv_r(r>1e-8) = 1./r(r>1e-8); + +cdist = ones(1,length(r)); +cdist(r > 1e-8) = theta_d(r > 1e-8) ./ r(r > 1e-8); + +if nargout > 1, + dcdistdom = [ ((inv_r').*(dtheta_ddom(:,1) - (cdist').*drdom(:,1))) ((inv_r').*(dtheta_ddom(:,2) - (cdist').*drdom(:,2))) ((inv_r').*(dtheta_ddom(:,3) - (cdist').*drdom(:,3))) ]; + dcdistdT = [ ((inv_r').*(dtheta_ddT(:,1) - (cdist').*drdT(:,1))) ((inv_r').*(dtheta_ddT(:,2) - (cdist').*drdT(:,2))) ((inv_r').*(dtheta_ddT(:,3) - (cdist').*drdT(:,3))) ]; + dcdistdk = [ (inv_r'.*dtheta_ddk(:,1)) (inv_r'.*dtheta_ddk(:,2)) (inv_r'.*dtheta_ddk(:,3)) (inv_r'.*dtheta_ddk(:,4)) ]; +end; + +xd1 = x .* (ones(2,1)*cdist); + +if nargout > 1, + dxd1dom = zeros(2*n,3); + dxd1dom(1:2:end,:) = (x(1,:)'*ones(1,3)) .* dcdistdom; + dxd1dom(2:2:end,:) = (x(2,:)'*ones(1,3)) .* dcdistdom; + coeff = (reshape([cdist;cdist],2*n,1)*ones(1,3)); + dxd1dom = dxd1dom + coeff.* dxdom; + + dxd1dT = zeros(2*n,3); + dxd1dT(1:2:end,:) = (x(1,:)'*ones(1,3)) .* dcdistdT; + dxd1dT(2:2:end,:) = (x(2,:)'*ones(1,3)) .* dcdistdT; + dxd1dT = dxd1dT + coeff.* dxdT; + + dxd1dk = zeros(2*n,4); + dxd1dk(1:2:end,:) = (x(1,:)'*ones(1,4)) .* dcdistdk; + dxd1dk(2:2:end,:) = (x(2,:)'*ones(1,4)) .* dcdistdk; +end; + +% No tangential distortion: +xd2 = xd1; +if nargout > 1, + dxd2dom = dxd1dom; + dxd2dT = dxd1dT; + dxd2dk = dxd1dk; +end; + +% Add Skew: +xd3 = [xd2(1,:) + alpha*xd2(2,:);xd2(2,:)]; + +% Compute: dxd3dom, dxd3dT, dxd3dk, dxd3dalpha +if nargout > 1, + dxd3dom = zeros(2*n,3); + dxd3dom(1:2:2*n,:) = dxd2dom(1:2:2*n,:) + alpha*dxd2dom(2:2:2*n,:); + dxd3dom(2:2:2*n,:) = dxd2dom(2:2:2*n,:); + dxd3dT = zeros(2*n,3); + dxd3dT(1:2:2*n,:) = dxd2dT(1:2:2*n,:) + alpha*dxd2dT(2:2:2*n,:); + dxd3dT(2:2:2*n,:) = dxd2dT(2:2:2*n,:); + dxd3dk = zeros(2*n,4); + dxd3dk(1:2:2*n,:) = dxd2dk(1:2:2*n,:) + alpha*dxd2dk(2:2:2*n,:); + dxd3dk(2:2:2*n,:) = dxd2dk(2:2:2*n,:); + dxd3dalpha = zeros(2*n,1); + dxd3dalpha(1:2:2*n,:) = xd2(2,:)'; +end; + +% Pixel coordinates: +if length(f)>1, + xp = xd3 .* (f * ones(1,n)) + c*ones(1,n); + if nargout > 1, + coeff = reshape(f*ones(1,n),2*n,1); + dxpdom = (coeff*ones(1,3)) .* dxd3dom; + dxpdT = (coeff*ones(1,3)) .* dxd3dT; + dxpdk = (coeff*ones(1,4)) .* dxd3dk; + dxpdalpha = (coeff) .* dxd3dalpha; + dxpdf = zeros(2*n,2); + dxpdf(1:2:end,1) = xd3(1,:)'; + dxpdf(2:2:end,2) = xd3(2,:)'; + end; +else + xp = f * xd3 + c*ones(1,n); + if nargout > 1, + dxpdom = f * dxd3dom; + dxpdT = f * dxd3dT; + dxpdk = f * dxd3dk; + dxpdalpha = f .* dxd3dalpha; + dxpdf = xd3(:); + end; +end; + +if nargout > 1, + dxpdc = zeros(2*n,2); + dxpdc(1:2:end,1) = ones(n,1); + dxpdc(2:2:end,2) = ones(n,1); +end; + +return; + +% Test of the Jacobians: + +n = 10; + +X = 10*randn(3,n); +om = randn(3,1); +T = [10*randn(2,1);40]; +f = 1000*rand(2,1); +c = 1000*randn(2,1); +k = 0.5*randn(4,1); +alpha = 0.01*randn(1,1); + +[x,dxdom,dxdT,dxdf,dxdc,dxdk,dxdalpha] = project_points_fisheye(X,om,T,f,c,k,alpha); + + +% Test on om: not OK + +dom = 0.00000000001 * norm(om)*randn(3,1); +om2 = om + dom; + +[x2] = project_points_fisheye(X,om2,T,f,c,k,alpha); + +x_pred = x + reshape(dxdom * dom,2,n); + + +norm(x2-x)/norm(x2 - x_pred) + + +% Test on T: not OK + +dT = 0.0001 * norm(T)*randn(3,1); +T2 = T + dT; + +[x2] = project_points_fisheye(X,om,T2,f,c,k,alpha); + +x_pred = x + reshape(dxdT * dT,2,n); + + +norm(x2-x)/norm(x2 - x_pred) + + + +% Test on f: OK!! + +df = 0.001 * norm(f)*randn(2,1); +f2 = f + df; + +[x2] = project_points_fisheye(X,om,T,f2,c,k,alpha); + +x_pred = x + reshape(dxdf * df,2,n); + + +norm(x2-x)/norm(x2 - x_pred) + + +% Test on c: OK!! + +dc = 0.01 * norm(c)*randn(2,1); +c2 = c + dc; + +[x2] = project_points_fisheye(X,om,T,f,c2,k,alpha); + +x_pred = x + reshape(dxdc * dc,2,n); + +norm(x2-x)/norm(x2 - x_pred) + +% Test on k: OK!! + +dk = 0.00001 * norm(k)*randn(4,1); +k2 = k + dk; + +[x2] = project_points_fisheye(X,om,T,f,c,k2,alpha); + +x_pred = x + reshape(dxdk * dk,2,n); + +norm(x2-x)/norm(x2 - x_pred) + + +% Test on alpha: OK!! + +dalpha = 0.001 * norm(k)*randn(1,1); +alpha2 = alpha + dalpha; + +[x2] = project_points_fisheye(X,om,T,f,c,k,alpha2); + +x_pred = x + reshape(dxdalpha * dalpha,2,n); + +norm(x2-x)/norm(x2 - x_pred) diff --git a/src/g_calib/project_points_weak.m b/src/g_calib/project_points_weak.m new file mode 100755 index 0000000000000000000000000000000000000000..b29b8ef8bfd3b5319ca73912970bf20b58d957d8 --- /dev/null +++ b/src/g_calib/project_points_weak.m @@ -0,0 +1,349 @@ +function [xp,dxpdom,dxpdT,dxpdf,dxpdc,dxpdk,dxpdalpha,Zave] = project_points_weak(X,om,T,f,c,k,alpha,Zave) + +%project_points_weak.m +% +%[xp,dxpdom,dxpdT,dxpdf,dxpdc,dxpdk] = project_points_weak(X,om,T,f,c,k,alpha) +% +%Projects a 3D structure onto the image plane. Weak perspective model used. +% +%INPUT: X: 3D structure in the world coordinate frame (3xN matrix for N points) +% (om,T): Rigid motion parameters between world coordinate frame and camera reference frame +% om: rotation vector (3x1 vector); T: translation vector (3x1 vector) +% f: camera focal length in units of horizontal and vertical pixel units (2x1 vector) +% c: principal point location in pixel units (2x1 vector) +% k: Distortion coefficients (radial and tangential) (4x1 vector) +% alpha: Skew coefficient between x and y pixel (alpha = 0 <=> square pixels) +% Zave: The average distance of the structure to the camera (opt). +% Default: computed average Z of the structure X in the camera reference frame (if not there) +% +%OUTPUT: xp: Projected pixel coordinates (2xN matrix for N points) +% dxpdom: Derivative of xp with respect to om ((2N)x3 matrix) +% dxpdT: Derivative of xp with respect to T ((2N)x3 matrix) +% dxpdf: Derivative of xp with respect to f ((2N)x2 matrix if f is 2x1, or (2N)x1 matrix is f is a scalar) +% dxpdc: Derivative of xp with respect to c ((2N)x2 matrix) +% dxpdk: Derivative of xp with respect to k ((2N)x4 matrix) +% +%Definitions: +%Let P be a point in 3D of coordinates X in the world reference frame (stored in the matrix X) +%The coordinate vector of P in the camera reference frame is: Xc = R*X + T +%where R is the rotation matrix corresponding to the rotation vector om: R = rodrigues(om); +%call x, y and z the 3 coordinates of Xc: x = Xc(1); y = Xc(2); z = Xc(3); +%The pinehole weak perspective projection coordinates of P is [a;b] where a=x/Zave and b=y/Zave. +%call r^2 = a^2 + b^2. +%The distorted point coordinates are: xd = [xx;yy] where: +% +%xx = a * (1 + kc(1)*r^2 + kc(2)*r^4 + kc(5)*r^6) + 2*kc(3)*a*b + kc(4)*(r^2 + 2*a^2); +%yy = b * (1 + kc(1)*r^2 + kc(2)*r^4 + kc(5)*r^6) + kc(3)*(r^2 + 2*b^2) + 2*kc(4)*a*b; +% +%The left terms correspond to radial distortion (6th degree), the right terms correspond to tangential distortion +% +%Finally, convertion into pixel coordinates: The final pixel coordinates vector xp=[xxp;yyp] where: +% +%xxp = f(1)*(xx + alpha*yy) + c(1) +%yyp = f(2)*yy + c(2) +% +% +%NOTE: About 90 percent of the code takes care fo computing the Jacobian matrices +% +% +%Important function called within that program: +% +%rodrigues.m: Computes the rotation matrix corresponding to a rotation vector +% +%rigid_motion.m: Computes the rigid motion transformation of a given structure + + +if nargin < 7, + alpha = 0; + if nargin < 6, + k = zeros(5,1); + if nargin < 5, + c = zeros(2,1); + if nargin < 4, + f = ones(2,1); + if nargin < 3, + T = zeros(3,1); + if nargin < 2, + om = zeros(3,1); + if nargin < 1, + error('Need at least a 3D structure to project (in project_points.m)'); + return; + end; + end; + end; + end; + end; + end; +end; + + +[m,n] = size(X); + +[Y,dYdom,dYdT] = rigid_motion(X,om,T); + +if ~exist('Zave'), + Zave = mean(Y(3,:)); +else + if Zave == 0, + Zave = mean(Y(3,:)); + end; +end; + +if 0 + inv_Z = 1./Y(3,:); +else + inv_Z = repmat(1/Zave,[1 n]); +end; + +x = (Y(1:2,:) .* (ones(2,1) * inv_Z)); + + + +if 0, + + bb = (-x(1,:) .* inv_Z)'*ones(1,3); + cc = (-x(2,:) .* inv_Z)'*ones(1,3); + + dxdom = zeros(2*n,3); + dxdom(1:2:end,:) = ((inv_Z')*ones(1,3)) .* dYdom(1:3:end,:) + bb .* dYdom(3:3:end,:); + dxdom(2:2:end,:) = ((inv_Z')*ones(1,3)) .* dYdom(2:3:end,:) + cc .* dYdom(3:3:end,:); + + dxdT = zeros(2*n,3); + dxdT(1:2:end,:) = ((inv_Z')*ones(1,3)) .* dYdT(1:3:end,:) + bb .* dYdT(3:3:end,:); + dxdT(2:2:end,:) = ((inv_Z')*ones(1,3)) .* dYdT(2:3:end,:) + cc .* dYdT(3:3:end,:); + +else + + + dxdom = zeros(2*n,3); + dxdom(1:2:end,:) = dYdom(1:3:end,:)/Zave; + dxdom(2:2:end,:) = dYdom(2:3:end,:)/Zave; + + dxdT = zeros(2*n,3); + dxdT(1:2:end,:) = dYdT(1:3:end,:)/Zave; + dxdT(2:2:end,:) = dYdT(2:3:end,:)/Zave; + +end; + + +% Add distortion: + +r2 = x(1,:).^2 + x(2,:).^2; + +dr2dom = 2*((x(1,:)')*ones(1,3)) .* dxdom(1:2:end,:) + 2*((x(2,:)')*ones(1,3)) .* dxdom(2:2:end,:); +dr2dT = 2*((x(1,:)')*ones(1,3)) .* dxdT(1:2:end,:) + 2*((x(2,:)')*ones(1,3)) .* dxdT(2:2:end,:); + + +r4 = r2.^2; + +dr4dom = 2*((r2')*ones(1,3)) .* dr2dom; +dr4dT = 2*((r2')*ones(1,3)) .* dr2dT; + + +r6 = r2.^3; + +dr6dom = 3*((r2'.^2)*ones(1,3)) .* dr2dom; +dr6dT = 3*((r2'.^2)*ones(1,3)) .* dr2dT; + + +% Radial distortion: + +cdist = 1 + k(1) * r2 + k(2) * r4 + k(5) * r6; + +dcdistdom = k(1) * dr2dom + k(2) * dr4dom + k(5) * dr6dom; +dcdistdT = k(1) * dr2dT + k(2) * dr4dT + k(5) * dr6dT; +dcdistdk = [ r2' r4' zeros(n,2) r6']; + + +xd1 = x .* (ones(2,1)*cdist); + +dxd1dom = zeros(2*n,3); +dxd1dom(1:2:end,:) = (x(1,:)'*ones(1,3)) .* dcdistdom; +dxd1dom(2:2:end,:) = (x(2,:)'*ones(1,3)) .* dcdistdom; +coeff = (reshape([cdist;cdist],2*n,1)*ones(1,3)); +dxd1dom = dxd1dom + coeff.* dxdom; + +dxd1dT = zeros(2*n,3); +dxd1dT(1:2:end,:) = (x(1,:)'*ones(1,3)) .* dcdistdT; +dxd1dT(2:2:end,:) = (x(2,:)'*ones(1,3)) .* dcdistdT; +dxd1dT = dxd1dT + coeff.* dxdT; + +dxd1dk = zeros(2*n,5); +dxd1dk(1:2:end,:) = (x(1,:)'*ones(1,5)) .* dcdistdk; +dxd1dk(2:2:end,:) = (x(2,:)'*ones(1,5)) .* dcdistdk; + + + +% tangential distortion: + +a1 = 2.*x(1,:).*x(2,:); +a2 = r2 + 2*x(1,:).^2; +a3 = r2 + 2*x(2,:).^2; + +delta_x = [k(3)*a1 + k(4)*a2 ; + k(3) * a3 + k(4)*a1]; + + +%ddelta_xdx = zeros(2*n,2*n); +aa = (2*k(3)*x(2,:)+6*k(4)*x(1,:))'*ones(1,3); +bb = (2*k(3)*x(1,:)+2*k(4)*x(2,:))'*ones(1,3); +cc = (6*k(3)*x(2,:)+2*k(4)*x(1,:))'*ones(1,3); + +ddelta_xdom = zeros(2*n,3); +ddelta_xdom(1:2:end,:) = aa .* dxdom(1:2:end,:) + bb .* dxdom(2:2:end,:); +ddelta_xdom(2:2:end,:) = bb .* dxdom(1:2:end,:) + cc .* dxdom(2:2:end,:); + +ddelta_xdT = zeros(2*n,3); +ddelta_xdT(1:2:end,:) = aa .* dxdT(1:2:end,:) + bb .* dxdT(2:2:end,:); +ddelta_xdT(2:2:end,:) = bb .* dxdT(1:2:end,:) + cc .* dxdT(2:2:end,:); + +ddelta_xdk = zeros(2*n,5); +ddelta_xdk(1:2:end,3) = a1'; +ddelta_xdk(1:2:end,4) = a2'; +ddelta_xdk(2:2:end,3) = a3'; +ddelta_xdk(2:2:end,4) = a1'; + + + +xd2 = xd1 + delta_x; + +dxd2dom = dxd1dom + ddelta_xdom ; +dxd2dT = dxd1dT + ddelta_xdT; +dxd2dk = dxd1dk + ddelta_xdk ; + + +% Add Skew: + +xd3 = [xd2(1,:) + alpha*xd2(2,:);xd2(2,:)]; + +% Compute: dxd3dom, dxd3dT, dxd3dk, dxd3dalpha + +dxd3dom = zeros(2*n,3); +dxd3dom(1:2:2*n,:) = dxd2dom(1:2:2*n,:) + alpha*dxd2dom(2:2:2*n,:); +dxd3dom(2:2:2*n,:) = dxd2dom(2:2:2*n,:); +dxd3dT = zeros(2*n,3); +dxd3dT(1:2:2*n,:) = dxd2dT(1:2:2*n,:) + alpha*dxd2dT(2:2:2*n,:); +dxd3dT(2:2:2*n,:) = dxd2dT(2:2:2*n,:); +dxd3dk = zeros(2*n,5); +dxd3dk(1:2:2*n,:) = dxd2dk(1:2:2*n,:) + alpha*dxd2dk(2:2:2*n,:); +dxd3dk(2:2:2*n,:) = dxd2dk(2:2:2*n,:); +dxd3dalpha = zeros(2*n,1); +dxd3dalpha(1:2:2*n,:) = xd2(2,:)'; + + + +% Pixel coordinates: +if length(f)>1, + xp = xd3 .* (f * ones(1,n)) + c*ones(1,n); + coeff = reshape(f*ones(1,n),2*n,1); + dxpdom = (coeff*ones(1,3)) .* dxd3dom; + dxpdT = (coeff*ones(1,3)) .* dxd3dT; + dxpdk = (coeff*ones(1,5)) .* dxd3dk; + dxpdalpha = (coeff) .* dxd3dalpha; + dxpdf = zeros(2*n,2); + dxpdf(1:2:end,1) = xd3(1,:)'; + dxpdf(2:2:end,2) = xd3(2,:)'; +else + xp = f * xd3 + c*ones(1,n); + dxpdom = f * dxd3dom; + dxpdT = f * dxd3dT; + dxpdk = f * dxd3dk; + dxpdalpha = f .* dxd3dalpha; + dxpdf = xd3(:); +end; + +dxpdc = zeros(2*n,2); +dxpdc(1:2:end,1) = ones(n,1); +dxpdc(2:2:end,2) = ones(n,1); + + +return; + +% Test of the Jacobians: + +n = 10; + +X = 10*randn(3,n); X(3,:) = X(3,:) + 100; +om = zeros(3,1); +T = zeros(3,1); +f = 1000*rand(2,1); +c = 1000*randn(2,1); +k = 0.5*randn(5,1); +alpha = 0.01*randn(1,1); + +[x,dxdom,dxdT,dxdf,dxdc,dxdk,dxdalpha] = project_points_weak(X,om,T,f,c,k,alpha); + + +% Test on om: OK + +dom = 0.0000001 *randn(3,1); +om2 = om + dom; + +[x2] = project_points_weak(X,om2,T,f,c,k,alpha); + +x_pred = x + reshape(dxdom * dom,2,n); + + +norm(x2-x)/norm(x2 - x_pred) + + +% Test on T: OK!! + +dT = 0.000000001 *randn(3,1); +T2 = T + dT; + +[x2] = project_points_weak(X,om,T2,f,c,k,alpha); + +x_pred = x + reshape(dxdT * dT,2,n); + + +norm(x2-x)/norm(x2 - x_pred) + + + +% Test on f: OK!! + +df = 0.001 * norm(f)*randn(2,1); +f2 = f + df; + +[x2] = project_points_weak(X,om,T,f2,c,k,alpha); + +x_pred = x + reshape(dxdf * df,2,n); + + +norm(x2-x)/norm(x2 - x_pred) + + +% Test on c: OK!! + +dc = 0.01 * norm(c)*randn(2,1); +c2 = c + dc; + +[x2] = project_points_weak(X,om,T,f,c2,k,alpha); + +x_pred = x + reshape(dxdc * dc,2,n); + +norm(x2-x)/norm(x2 - x_pred) + +% Test on k: OK!! + +dk = 0.001 * norm(k)*randn(5,1); +k2 = k + dk; + +[x2] = project_points_weak(X,om,T,f,c,k2,alpha); + +x_pred = x + reshape(dxdk * dk,2,n); + +norm(x2-x)/norm(x2 - x_pred) + + +% Test on alpha: OK!! + +dalpha = 0.001 * norm(k)*randn(1,1); +alpha2 = alpha + dalpha; + +[x2] = project_points_weak(X,om,T,f,c,k,alpha2); + +x_pred = x + reshape(dxdalpha * dalpha,2,n); + +norm(x2-x)/norm(x2 - x_pred) diff --git a/src/g_calib/projectedGrid.m b/src/g_calib/projectedGrid.m new file mode 100755 index 0000000000000000000000000000000000000000..561a7d0b2c83ed59967ae5b8d41ccb5cc0b1ff58 --- /dev/null +++ b/src/g_calib/projectedGrid.m @@ -0,0 +1,24 @@ +function [XX,H] = projectedGrid ( P1, P2, P3, P4 , nx, ny); + +% new formalism using homographies + +a00 = [P1;1]; +a10 = [P2;1]; +a11 = [P3;1]; +a01 = [P4;1]; + +% Compute the planart collineation: + +[H] = compute_collineation (a00, a10, a11, a01); + + +% Build the grid using the planar collineation: + +x_l = ((0:(nx-1))'*ones(1,ny))/(nx-1); +y_l = (ones(nx,1)*(0:(ny-1)))/(ny-1); + +pts = [x_l(:) y_l(:) ones(nx*ny,1)]'; + +XX = H*pts; + +XX = XX(1:2,:) ./ (ones(2,1)*XX(3,:)); diff --git a/src/g_calib/projector_calib.m b/src/g_calib/projector_calib.m new file mode 100755 index 0000000000000000000000000000000000000000..11a83cdca8780df82b4f2ec436f208fc98e618dc --- /dev/null +++ b/src/g_calib/projector_calib.m @@ -0,0 +1,93 @@ + +% load camera results (for setting active images, n_ima,...) +load camera_results; + + +% Projection settings: + +wintx = 12; %8; +winty = 12; %8; +nx = 1024; +ny = 768; +dX = 32; +dY = 32; +dXoff=511.5; +dYoff=383.5; + + +proj_name = 'proj'; %input('Basename projector calibration images (without number nor suffix): ','s'); +camera_name = 'cam'; + + +xr_list = NaN*ones(1,n_ima); +yr_list = NaN*ones(1,n_ima); + + +ind_proc = ind_active; + + + +DEBUG = 1; +%ind_ima_proj = [18]; + +for i = ind_proc, + + projector_marker; + +end; + + +fprintf(1,'\nExtraction of the grid corners on the image\n'); + + + +recompute_corner = 0; + +if recompute_corner & ~exist(['xproj_' num2str(ind_proc(1))]), + if exist('projector_data'), + load projector_data; + else + recompute_corner = 0; + disp('WARNING: Cannot recompute corners. Data need to be extracted at least once'); + end; +end; + +if ~recompute_corner, + disp('Manual extraction mode'); +else + disp('Automatic recomputation of the corners'); +end; + +% extract the projector corners: + + +for kk = ind_proc, + + projector_ima_corners + +end; + + +string_save = 'save projector_data '; + +for kk = ind_active, + string_save = [string_save ' xproj_' num2str(kk) ' x_proj_' num2str(kk)]; +end; +eval(string_save); + + + + +return; + + +i = 18; + +figure(4) +image(I_29); +colormap(gray(256)); +hold on; +plot(x_29(1,:)+1,x_29(2,:)+1,'r+','markersize',13,'linewidth',3) +hold off; +axis image +axis off; diff --git a/src/g_calib/projector_ima_corners.m b/src/g_calib/projector_ima_corners.m new file mode 100755 index 0000000000000000000000000000000000000000..6b18cf9b8e72a9d0a8f15fc35f07c5af0126658d --- /dev/null +++ b/src/g_calib/projector_ima_corners.m @@ -0,0 +1,61 @@ + + eval(['Ip = Ip_' num2str(kk) ';']); + eval(['In = In_' num2str(kk) ';']); + + xr = xr_list(kk); + yr = yr_list(kk); + + fprintf(1,'Processing image %d...',kk); + + if ~recompute_corner, + [x,X,n_sq_x,n_sq_y,ind_orig,ind_x,ind_y] = extract_grid(Ip,wintx,winty,fc_save,cc_save,kc_save,dX,dY,xr,yr,1); + xproj = x; + else + eval(['xproj = xproj_' num2str(kk) ';']); + x = cornerfinder(xproj+1,Ip,winty,wintx); + xproj = x - 1; + end; + + Np_proj = size(x,2); + + figure(2); + image(Ip); + hold on; + plot(xproj(1,:)+1,xproj(2,:)+1,'r+'); + %title('Click on your reference point'); + xlabel('Xc (in camera frame)'); + ylabel('Yc (in camera frame)'); + hold off; + + %disp('Click on your reference point...'); + + %[xr,yr] = ginput2(1); + + xr = xr_list(kk); + yr = yr_list(kk); + + err = sqrt(sum((xproj - [xr;yr]*ones(1,Np_proj)).^2)); + ind_ref = find(err == min(err)); + + ref_pt = xproj(:,ind_ref); + + figure(2); + hold on; + plot(ref_pt(1)+1,ref_pt(2)+1,'go'); hold off; + title(['Image ' num2str(kk)]); + drawnow; + + + if ~recompute_corner, + + off_x = mod(ind_ref-1,n_sq_x+1); + off_y = n_sq_y - floor((ind_ref-1)/(n_sq_x+1)); + + x_proj = X(1:2,:) + ([dXoff - dX * off_x ; dYoff - dY * off_y]*ones(1,Np_proj)); + + eval(['x_proj_' num2str(kk) ' = x_proj;']); % coordinates of the points in the projector image + + end; + + eval(['xproj_' num2str(kk) ' = xproj;']); % coordinates of the points in the camera image + diff --git a/src/g_calib/projector_marker.m b/src/g_calib/projector_marker.m new file mode 100755 index 0000000000000000000000000000000000000000..2c34c95228518c1e0496551d661fc9b032913c8c --- /dev/null +++ b/src/g_calib/projector_marker.m @@ -0,0 +1,146 @@ + if active_images(i), + + %fprintf(1,'Loading image %d...\n',i); + + if ~type_numbering, + number_ext = num2str(image_numbers(i)); + else + number_ext = sprintf(['%.' num2str(N_slots) 'd'],image_numbers(i)); + end; + + ima_namep = [proj_name number_ext 'p.' format_image]; + ima_namen = [proj_name number_ext 'n.' format_image]; + ima_namer = [proj_name number_ext 'r.' format_image]; + ima_nameb = [camera_name number_ext '.' format_image]; + + if i == ind_active(1), + fprintf(1,'Loading image '); + end; + + fprintf(1,'%d...',i); + + if format_image(1) == 'p', + if format_image(2) == 'p', + Ip = double(loadppm(ima_namep)); + In = double(loadppm(ima_namen)); + Ir = double(loadppm(ima_namer)); + Ib = double(loadppm(ima_nameb)); + else + Ip = double(loadpgm(ima_namep)); + In = double(loadpgm(ima_namen)); + Ir = double(loadpgm(ima_namer)); + Ib = double(loadpgm(ima_nameb)); + end; + else + if format_image(1) == 'r', + Ip = readras(ima_namep); + In = readras(ima_namen); + Ir = readras(ima_namer); + Ib = readras(ima_nameb); + else + Ip = double(imread(ima_namep)); + In = double(imread(ima_namen)); + Ir = double(imread(ima_namer)); + Ib = double(imread(ima_nameb)); + end; + end; + + + if size(Ip,3)>1, + Ip = 0.299 * Ip(:,:,1) + 0.5870 * Ip(:,:,2) + 0.114 * Ip(:,:,3); + In = 0.299 * In(:,:,1) + 0.5870 * In(:,:,2) + 0.114 * In(:,:,3); + Ir = 0.299 * Ir(:,:,1) + 0.5870 * Ir(:,:,2) + 0.114 * Ir(:,:,3); + Ib = 0.299 * Ib(:,:,1) + 0.5870 * Ib(:,:,2) + 0.114 * Ib(:,:,3); + end; + + %IIp = Ip - In; + + %Ip2 = Ip - Ib; + %In2 = In - Ib; + + %imax = max(IIp(:)); + %imin = min(IIp(:)); + + %IIp = 255*(IIp - imin)/(imax - imin); + + %indplus = find(IIp >= 255/2); + %indminus = find(IIp < 255/2); + + %IIp(indplus) = 255*ones(length(indplus),1); + %IIp(indminus) = zeros(length(indminus),1); + + delta_I = Ip - In; + + %IIp = 255*(1 + exp(-delta_I/2)).^(-1); + + + IIp = (Ip >= In)*255; + + IIp = conv2(conv2(IIp,[1/4 1/2 1/4],'same'),[1/4 1/2 1/4]','same'); + + if 0, + figure(4); + image(IIp); + colormap(gray(256)); + axis off; + end; + + + eval(['Ip_' num2str(i) ' = IIp;']); + eval(['In_' num2str(i) ' = 255 - IIp;']); + + + + + I_marker2 = Ib-Ir; + + + if DEBUG, + + Imax = max(I_marker2(:)); + Imin = min(I_marker2(:)); + + I_marker_out = 255*(I_marker2 - Imin)/(Imax - Imin); + + figure(3); + image(I_marker_out); + colormap(gray(256)); + + [xr,yr] = ginput(1); + + xr_list(i) = xr; + yr_list(i) = yr; + + else + + I_marker = I_marker2 >50; + I_marker = eliminate_boundary(I_marker); + I_marker = eliminate_boundary(I_marker); + [ymm,xmm] = find(I_marker); + + if length(xmm)<10, + fprintf(1,'WARNING!! No marker in image %d!!!!\n',i); + else + xr = mean(xmm); + yr = mean(ymm); + xr_list(i) = xr; + yr_list(i) = yr; + end; + end; + + figure(2); + image(IIp); + colormap(gray(256)); + hold on; + plot(xr,yr,'go'); + hold off; + title(['Image ' num2str(i)]); + drawnow; + + if ~DEBUG + waitforbuttonpress; + end; + + + + end; diff --git a/src/g_calib/readras.m b/src/g_calib/readras.m new file mode 100755 index 0000000000000000000000000000000000000000..37e9c496bad2757facddc742360355fc90b499be --- /dev/null +++ b/src/g_calib/readras.m @@ -0,0 +1,87 @@ +function [X, map] = readras(filename, ys, ye, xs, xe); +%READRAS Read an image file in sun raster format. +% READRAS('imagefile.ras') reads a "sun.raster" image file. +% [X, map] = READRAS('imagefile.ras') returns both the image and a +% color map, so that +% [X, map] = readras('imagefile.ras'); +% image(X) +% colormap(map) +% axis('equal') +% will display the result with the proper colors. +% NOTE: readras cannot deal with complicated color maps. +% In fact, Matlab doesn't quite allow to work with colormaps +% with more than 64 entries. +% + +%% +%% (C) Thomas K. Leung 3/30/93. +%% California Institute of Technology. +%% Modified by Andrea Mennucci to deal with color images +%% + +% PC and UNIX version of readras - Jean-Yves Bouguet - Dec. 1998 + +dot = max(find(filename == '.')); +suffix = filename(dot+1:dot+3); + +if(strcmp(lower(suffix), 'ras')) % raster file format % + fp = fopen(filename, 'rb'); + if(fp<0) error(['Cannot open ' filename '.']), end + + %Read and crack the 32-byte header + fseek(fp, 4, -1); + + width = 2^24 * fread(fp, 1, 'uchar') + 2^16 * fread(fp, 1, 'uchar') + 2^8 * fread(fp, 1, 'uchar') + fread(fp, 1, 'uchar'); + + height = 2^24 * fread(fp, 1, 'uchar') + 2^16 * fread(fp, 1, 'uchar') + 2^8 * fread(fp, 1, 'uchar') + fread(fp, 1, 'uchar'); + + depth = 2^24 * fread(fp, 1, 'uchar') + 2^16 * fread(fp, 1, 'uchar') + 2^8 * fread(fp, 1, 'uchar') + fread(fp, 1, 'uchar'); + + length = 2^24 * fread(fp, 1, 'uchar') + 2^16 * fread(fp, 1, 'uchar') + 2^8 * fread(fp, 1, 'uchar') + fread(fp, 1, 'uchar'); + + type = 2^24 * fread(fp, 1, 'uchar') + 2^16 * fread(fp, 1, 'uchar') + 2^8 * fread(fp, 1, 'uchar') + fread(fp, 1, 'uchar'); + + maptype = 2^24 * fread(fp, 1, 'uchar') + 2^16 * fread(fp, 1, 'uchar') + 2^8 * fread(fp, 1, 'uchar') + fread(fp, 1, 'uchar'); + + maplen = 2^24 * fread(fp, 1, 'uchar') + 2^16 * fread(fp, 1, 'uchar') + 2^8 * fread(fp, 1, 'uchar') + fread(fp, 1, 'uchar'); + + maplen = maplen / 3; + + if maptype == 2 % RMT_RAW + map = fread(fp, [maplen, 3], 'uchar')/255; +% if maplen<64, map=[map',zeros(3,64-maplen)]';maplen=64; end; + elseif maptype == 1 % RMT_EQUAL_RGB + map(:,1) = fread(fp, [maplen], 'uchar'); + map(:,2) = fread(fp, [maplen], 'uchar'); + map(:,3) = fread(fp, [maplen], 'uchar'); + %maxmap = max(max(map)); + map = map/255; + if maplen<64, map=[map',zeros(3,64-maplen)]'; maplen=64; end; + else % RMT_NONE + map = []; + end +% if maplen>64, +% map=[map',zeros(3,256-maplen)]'; +% end; + + % Read the image + + if rem(width,2) == 1 + Xt = fread(fp, [width+1, height], 'uchar'); + X = Xt(1:width, :)'; + else + Xt = fread(fp, [width, height], 'uchar'); + X = Xt'; + end + X = X + 1; + fclose(fp); +else + error('Image file name must end in either ''ras'' or ''rast''.'); +end + + +if nargin == 5 + + X = X(ys:ye, xs:xe); + +end \ No newline at end of file diff --git a/src/g_calib/recomp_corner_calib.m b/src/g_calib/recomp_corner_calib.m new file mode 100755 index 0000000000000000000000000000000000000000..7c396936dcae95ec1853264d8e042760610bf3b8 --- /dev/null +++ b/src/g_calib/recomp_corner_calib.m @@ -0,0 +1,130 @@ +% Re-select te corners after calibration + +if ~exist('n_ima')|~exist('fc'), + fprintf(1,'No calibration data available.\n'); + return; +end; + +check_active_images; + +flag = 0; +for kk = ind_active, + if ~exist(['y_' num2str(kk)]), + flag = 1; + else + eval(['ykk = y_' num2str(kk) ';']); + if isnan(ykk(1,1)), + flag = 1; + end; + end; +end; + +if flag, + fprintf(1,'Need to calibrate once before before recomputing image corners. Maybe need to load Calib_Results.mat file.\n'); + return; +end; + + +if n_ima == 0, + + fprintf(1,'No image data available\n'); + +else + +if ~exist(['I_' num2str(ind_active(1))]), + n_ima_save = n_ima; + active_images_save = active_images; + ima_read_calib; + n_ima = n_ima_save; + active_images = active_images_save; + check_active_images; + if no_image_file, + disp('Cannot extract corners without images'); + return; + end; +end; + +fprintf(1,'\nRe-extraction of the grid corners on the images (after first calibration)\n'); + +disp('Window size for corner finder (wintx and winty):'); +wintx = input('wintx ([] = 5) = '); +if isempty(wintx), wintx = 5; end; +wintx = round(wintx); +winty = input('winty ([] = 5) = '); +if isempty(winty), winty = 5; end; +winty = round(winty); + +fprintf(1,'Window size = %dx%d\n',2*wintx+1,2*winty+1); + +ima_numbers = input('Number(s) of image(s) to process ([] = all images) = '); + +if isempty(ima_numbers), + ima_proc = 1:n_ima; +else + ima_proc = ima_numbers; +end; + +q_auto = input('Use the projection of 3D grid or manual click ([]=auto, other=manual): ','s'); + +fprintf(1,'Processing image '); + +for kk = ima_proc; + + if active_images(kk), + + fprintf(1,'%d...',kk); + + if isempty(q_auto), + + eval(['I = I_' num2str(kk) ';']); + + eval(['y = y_' num2str(kk) ';']); + + xc = cornerfinder(y+1,I,winty,wintx); % the four corners + + eval(['wintx_' num2str(kk) ' = wintx;']); + eval(['winty_' num2str(kk) ' = winty;']); + + eval(['x_' num2str(kk) '= xc - 1;']); + + else + + fprintf(1,'\n'); + + eval(['wintx_' num2str(kk) ' = wintx;']); + eval(['winty_' num2str(kk) ' = winty;']); + + click_ima_calib; + + end; + + else + + if ~exist(['omc_' num2str(kk)]), + + eval(['dX_' num2str(kk) ' = NaN;']); + eval(['dY_' num2str(kk) ' = NaN;']); + + eval(['wintx_' num2str(kk) ' = NaN;']); + eval(['winty_' num2str(kk) ' = NaN;']); + + eval(['x_' num2str(kk) ' = NaN*ones(2,1);']); + eval(['X_' num2str(kk) ' = NaN*ones(3,1);']); + + eval(['n_sq_x_' num2str(kk) ' = NaN;']); + eval(['n_sq_y_' num2str(kk) ' = NaN;']); + + end; + + end; + + +end; + +% Recompute the error: + +comp_error_calib; + +fprintf(1,'\ndone\n'); + +end; diff --git a/src/g_calib/recomp_corner_calib_fisheye_no_read.m b/src/g_calib/recomp_corner_calib_fisheye_no_read.m new file mode 100755 index 0000000000000000000000000000000000000000..d77bb52854aff8c836dfcfab3bac27877741fa73 --- /dev/null +++ b/src/g_calib/recomp_corner_calib_fisheye_no_read.m @@ -0,0 +1,141 @@ +% Re-select te corners after calibration + +if ~exist('n_ima')|~exist('fc'), + fprintf(1,'No calibration data available.\n'); + return; +end; + +check_active_images; + +flag = 0; +for kk = ind_active, + if ~exist(['y_' num2str(kk)]), + flag = 1; + else + eval(['ykk = y_' num2str(kk) ';']); + if isnan(ykk(1,1)), + flag = 1; + end; + end; +end; + +if flag, + fprintf(1,'Need to calibrate once before before recomputing image corners. Maybe need to load Calib_Results.mat file.\n'); + return; +end; + +if n_ima == 0, + fprintf(1,'No image data available\n'); + return; +end + +%if ~exist(['I_' num2str(ind_active(1))]), +% n_ima_save = n_ima; +% active_images_save = active_images; +% ima_read_calib; +% n_ima = n_ima_save; +% active_images = active_images_save; +% check_active_images; +% if no_image_file, +% disp('Cannot extract corners without images'); +% return; +% end; +%end; + +fprintf(1,'\nRe-extraction of the grid corners on the images (after first calibration)\n'); + +if ~dont_ask, + disp('Window size for corner finder (wintx and winty):'); + wintx = input('wintx ([] = 20) = '); + if isempty(wintx), wintx = 20; end; + wintx = round(wintx); + winty = input('winty ([] = 20) = '); + if isempty(winty), winty = 20; end; + winty = round(winty); +else + wintx = 20; + winty = 20; +end; + +fprintf(1,'Window size = %dx%d\n',2*wintx+1,2*winty+1); + +if ~dont_ask, + ima_numbers = input('Number(s) of image(s) to process ([] = all images) = '); +else + ima_numbers = []; +end; + +if isempty(ima_numbers), + ima_proc = 1:n_ima; +else + ima_proc = ima_numbers; +end; + +if ~dont_ask, + q_auto = input('Use the projection of 3D grid or manual click ([]=auto, other=manual): ','s'); +else + q_auto = []; +end; + +fprintf(1,'Processing image '); + +for kk = ima_proc; + if active_images(kk), + fprintf(1,'%d...',kk); + + if ~type_numbering, + number_ext = num2str(image_numbers(kk)); + else + number_ext = sprintf(['%.' num2str(N_slots) 'd'],image_numbers(kk)); + end; + ima_name = [calib_name number_ext '.' format_image]; + + if exist(ima_name), + if format_image(1) == 'p'; + if format_image(2) == 'p', + I = double(loadppm(ima_name)); + else + I = double(loadpgm(ima_name)); + end; + else + if format_image(1) == 'r', + I = readras(ima_name); + else + I = double(imread(ima_name)); + end; + end; + if size(I,3)>1, + I = 0.299 * I(:,:,1) + 0.5870 * I(:,:,2) + 0.114 * I(:,:,3); + end; + [ny,nx,junk] = size(I); + if isempty(q_auto), + eval(['y = y_' num2str(kk) ';']); + xc = cornerfinder(y+1,I,winty,wintx); % the four corners + eval(['wintx_' num2str(kk) ' = wintx;']); + eval(['winty_' num2str(kk) ' = winty;']); + eval(['x_' num2str(kk) '= xc - 1;']); + else + fprintf(1,'\n'); + fprintf(1,'\nProcessing image %d...\n',kk); + click_calib_fisheye_no_read; + end; + else + fprintf(1,'Image %s not found!!!...',ima_name); + end; + else + if ~exist(['omc_' num2str(kk)]), + eval(['dX_' num2str(kk) ' = NaN;']); + eval(['dY_' num2str(kk) ' = NaN;']); + eval(['wintx_' num2str(kk) ' = NaN;']); + eval(['winty_' num2str(kk) ' = NaN;']); + eval(['x_' num2str(kk) ' = NaN*ones(2,1);']); + eval(['X_' num2str(kk) ' = NaN*ones(3,1);']); + eval(['n_sq_x_' num2str(kk) ' = NaN;']); + eval(['n_sq_y_' num2str(kk) ' = NaN;']); + end; + end; +end; + +% Recompute the error: +comp_error_calib_fisheye; +fprintf(1,'\ndone\n'); diff --git a/src/g_calib/recomp_corner_calib_no_read.m b/src/g_calib/recomp_corner_calib_no_read.m new file mode 100755 index 0000000000000000000000000000000000000000..3e982cc7da9c4b6a778c1d69580a0b721fefdf7d --- /dev/null +++ b/src/g_calib/recomp_corner_calib_no_read.m @@ -0,0 +1,128 @@ +% Re-select te corners after calibration +if ~exist('n_ima')|~exist('fc'), + fprintf(1,'No calibration data available.\n'); + return; +end; + +check_active_images; + +flag = 0; +for kk = ind_active, + if ~exist(['y_' num2str(kk)]), + flag = 1; + else + eval(['ykk = y_' num2str(kk) ';']); + if isnan(ykk(1,1)), + flag = 1; + end; + end; +end; + +if flag, + fprintf(1,'Need to calibrate once before before recomputing image corners. Maybe need to load Calib_Results.mat file.\n'); + return; +end; + +if n_ima == 0, + fprintf(1,'No image data available\n'); + return; +end + +fprintf(1,'\nRe-extraction of the grid corners on the images (after first calibration)\n'); + +if ~exist('dont_ask'), + dont_ask = 0; +end; + +if dont_ask, + wintx = 20; + winty = 20; +else + disp('Window size for corner finder (wintx and winty):'); + wintx = input('wintx ([] = 20) = '); + if isempty(wintx), wintx = 20; end; + wintx = round(wintx); + winty = input('winty ([] = 20) = '); + if isempty(winty), winty = 20; end; + winty = round(winty); +end; + +fprintf(1,'Window size = %dx%d\n',2*wintx+1,2*winty+1); + +if dont_ask, + ima_numbers = []; +else + ima_numbers = input('Number(s) of image(s) to process ([] = all images) = '); +end; + +if isempty(ima_numbers), + ima_proc = 1:n_ima; +else + ima_proc = ima_numbers; +end; + +if dont_ask, + q_auto = []; +else + q_auto = input('Use the projection of 3D grid or manual click ([]=auto, other=manual): ','s'); +end; + +fprintf(1,'Processing image '); + +for kk = ima_proc; + if active_images(kk), + fprintf(1,'%d...',kk); + if ~type_numbering, + number_ext = num2str(image_numbers(kk)); + else + number_ext = sprintf(['%.' num2str(N_slots) 'd'],image_numbers(kk)); + end; + ima_name = [calib_name number_ext '.' format_image]; + if exist(ima_name), + if format_image(1) == 'p', + if format_image(2) == 'p', + I = double(loadppm(ima_name)); + else + I = double(loadpgm(ima_name)); + end; + else + if format_image(1) == 'r', + I = readras(ima_name); + else + I = double(imread(ima_name)); + end; + end; + if size(I,3)>1, + I = 0.299 * I(:,:,1) + 0.5870 * I(:,:,2) + 0.114 * I(:,:,3); + end; + [ny,nx,junk] = size(I); + if isempty(q_auto), + eval(['y = y_' num2str(kk) ';']); + xc = cornerfinder(y+1,I,winty,wintx); % the four corners + eval(['wintx_' num2str(kk) ' = wintx;']); + eval(['winty_' num2str(kk) ' = winty;']); + eval(['x_' num2str(kk) '= xc - 1;']); + else + fprintf(1,'\n'); + fprintf(1,'\nProcessing image %d...\n',kk); + click_ima_calib_no_read; + end; + else + fprintf(1,'Image %s not found!!!...',ima_name); + end; + else + if ~exist(['omc_' num2str(kk)]), + eval(['dX_' num2str(kk) ' = NaN;']); + eval(['dY_' num2str(kk) ' = NaN;']); + eval(['wintx_' num2str(kk) ' = NaN;']); + eval(['winty_' num2str(kk) ' = NaN;']); + eval(['x_' num2str(kk) ' = NaN*ones(2,1);']); + eval(['X_' num2str(kk) ' = NaN*ones(3,1);']); + eval(['n_sq_x_' num2str(kk) ' = NaN;']); + eval(['n_sq_y_' num2str(kk) ' = NaN;']); + end; + end; +end; +% Recompute the error: +comp_error_calib; +fprintf(1,'\ndone\n'); diff --git a/src/g_calib/recomp_corner_calib_saddle_points.m b/src/g_calib/recomp_corner_calib_saddle_points.m new file mode 100755 index 0000000000000000000000000000000000000000..10d3f051f80db409b897e1b1eba5895a1ba0b7ab --- /dev/null +++ b/src/g_calib/recomp_corner_calib_saddle_points.m @@ -0,0 +1,61 @@ +fprintf(1,'Recomputation of the image corners using Lucchese''s saddle point detector\n'); + +q = input('This detector only works for X junctions (checker board corners). Is this the case here? ([]=yes, other=no)','s'); + + +if isempty(q), + + ima_proc = 1:n_ima; + + fprintf(1,'Processing image '); + + for kk = ima_proc; + + if active_images(kk), + + fprintf(1,'%d...',kk); + + + eval(['I = I_' num2str(kk) ';']); + + eval(['y = x_' num2str(kk) ';']); + eval(['wintx = wintx_' num2str(kk) ';']); + eval(['winty = winty_' num2str(kk) ';']); + + xc = cornerfinder_saddle_point(y+1,I,winty,wintx); % the four corners + + eval(['x_' num2str(kk) '= xc - 1;']); + + else + + if ~exist(['omc_' num2str(kk)]), + + eval(['dX_' num2str(kk) ' = NaN;']); + eval(['dY_' num2str(kk) ' = NaN;']); + + eval(['wintx_' num2str(kk) ' = NaN;']); + eval(['winty_' num2str(kk) ' = NaN;']); + + eval(['x_' num2str(kk) ' = NaN*ones(2,1);']); + eval(['X_' num2str(kk) ' = NaN*ones(3,1);']); + + eval(['n_sq_x_' num2str(kk) ' = NaN;']); + eval(['n_sq_y_' num2str(kk) ' = NaN;']); + + end; + + end; + + end; + + % Recompute the error: + + comp_error_calib; + + fprintf(1,'\ndone\n'); + +else + + fprintf(1,'No recomputation done (The points are not locally saddle points)\n'); + +end; diff --git a/src/g_calib/rect.m b/src/g_calib/rect.m new file mode 100755 index 0000000000000000000000000000000000000000..54d0e0c77a371735ba5bd3384d5238130dd626b6 --- /dev/null +++ b/src/g_calib/rect.m @@ -0,0 +1,126 @@ +function [Irec] = rect(I,R,f,c,k,alpha,KK_new); + + +if nargin < 5, + k = [0;0;0;0;0]; + if nargin < 4, + c = [0;0]; + if nargin < 3, + f = [1;1]; + if nargin < 2, + R = eye(3); + if nargin < 1, + error('ERROR: Need an image to rectify'); + end; + end; + end; + end; +end; + + +if nargin < 7, + if nargin < 6, + KK_new = [f(1) 0 c(1);0 f(2) c(2);0 0 1]; + else + KK_new = alpha; % the 6th argument is actually KK_new + end; + alpha = 0; +end; + + + +% Note: R is the motion of the points in space +% So: X2 = R*X where X: coord in the old reference frame, X2: coord in the new ref frame. + + +if ~exist('KK_new'), + KK_new = [f(1) alpha*f(1) c(1);0 f(2) c(2);0 0 1]; +end; + + +[nr,nc] = size(I); + +Irec = 255*ones(nr,nc); + +[mx,my] = meshgrid(1:nc, 1:nr); +px = reshape(mx',nc*nr,1); +py = reshape(my',nc*nr,1); + +rays = inv(KK_new)*[(px - 1)';(py - 1)';ones(1,length(px))]; + + +% Rotation: (or affine transformation): + +rays2 = R'*rays; + +x = [rays2(1,:)./rays2(3,:);rays2(2,:)./rays2(3,:)]; + + +% Add distortion: +xd = apply_distortion(x,k); + + +% Reconvert in pixels: + +px2 = f(1)*(xd(1,:)+alpha*xd(2,:))+c(1); +py2 = f(2)*xd(2,:)+c(2); + + +% Interpolate between the closest pixels: + +px_0 = floor(px2); + + +py_0 = floor(py2); +py_1 = py_0 + 1; + + +good_points = find((px_0 >= 0) & (px_0 <= (nc-2)) & (py_0 >= 0) & (py_0 <= (nr-2))); + +px2 = px2(good_points); +py2 = py2(good_points); +px_0 = px_0(good_points); +py_0 = py_0(good_points); + +alpha_x = px2 - px_0; +alpha_y = py2 - py_0; + +a1 = (1 - alpha_y).*(1 - alpha_x); +a2 = (1 - alpha_y).*alpha_x; +a3 = alpha_y .* (1 - alpha_x); +a4 = alpha_y .* alpha_x; + +ind_lu = px_0 * nr + py_0 + 1; +ind_ru = (px_0 + 1) * nr + py_0 + 1; +ind_ld = px_0 * nr + (py_0 + 1) + 1; +ind_rd = (px_0 + 1) * nr + (py_0 + 1) + 1; + +ind_new = (px(good_points)-1)*nr + py(good_points); + + + +Irec(ind_new) = a1 .* I(ind_lu) + a2 .* I(ind_ru) + a3 .* I(ind_ld) + a4 .* I(ind_rd); + + + +return; + + +% Convert in indices: + +fact = 3; + +[XX,YY]= meshgrid(1:nc,1:nr); +[XXi,YYi]= meshgrid(1:1/fact:nc,1:1/fact:nr); + +%tic; +Iinterp = interp2(XX,YY,I,XXi,YYi); +%toc + +[nri,nci] = size(Iinterp); + + +ind_col = round(fact*(f(1)*xd(1,:)+c(1)))+1; +ind_row = round(fact*(f(2)*xd(2,:)+c(2)))+1; + +good_points = find((ind_col >=1)&(ind_col<=nci)&(ind_row >=1)& (ind_row <=nri)); diff --git a/src/g_calib/rect_index.m b/src/g_calib/rect_index.m new file mode 100755 index 0000000000000000000000000000000000000000..69928e32f0bac0455922057bfc289a70b366ee20 --- /dev/null +++ b/src/g_calib/rect_index.m @@ -0,0 +1,123 @@ +function [Irec,ind_new,ind_1,ind_2,ind_3,ind_4,a1,a2,a3,a4] = rect_index(I,R,f,c,k,alpha,KK_new); + + +if nargin < 5, + k = [0;0;0;0;0]; + if nargin < 4, + c = [0;0]; + if nargin < 3, + f = [1;1]; + if nargin < 2, + R = eye(3); + if nargin < 1, + error('ERROR: Need an image to rectify'); + %break; + end; + end; + end; + end; +end; + + +if nargin < 7, + if nargin < 6, + KK_new = [f(1) 0 c(1);0 f(2) c(2);0 0 1]; + else + KK_new = alpha; % the 6th argument is actually KK_new + end; + alpha = 0; +end; + + + +% Note: R is the motion of the points in space +% So: X2 = R*X where X: coord in the old reference frame, X2: coord in the new ref frame. + + +if ~exist('KK_new'), + KK_new = [f(1) alpha_c*fc(1) c(1);0 f(2) c(2);0 0 1]; +end; + + +[nr,nc] = size(I); + +Irec = 255*ones(nr,nc); + +[mx,my] = meshgrid(1:nc, 1:nr); +px = reshape(mx',nc*nr,1); +py = reshape(my',nc*nr,1); + +rays = inv(KK_new)*[(px - 1)';(py - 1)';ones(1,length(px))]; + + +% Rotation: (or affine transformation): + +rays2 = R'*rays; + +x = [rays2(1,:)./rays2(3,:);rays2(2,:)./rays2(3,:)]; + + +% Add distortion: +xd = apply_distortion(x,k); + + +% Reconvert in pixels: + +px2 = f(1)*(xd(1,:)+alpha*xd(2,:))+c(1); +py2 = f(2)*xd(2,:)+c(2); + + +% Interpolate between the closest pixels: + +px_0 = floor(px2); +py_0 = floor(py2); + + +good_points = find((px_0 >= 0) & (px_0 <= (nc-2)) & (py_0 >= 0) & (py_0 <= (nr-2))); + +px2 = px2(good_points); +py2 = py2(good_points); +px_0 = px_0(good_points); +py_0 = py_0(good_points); + +alpha_x = px2 - px_0; +alpha_y = py2 - py_0; + +a1 = (1 - alpha_y).*(1 - alpha_x); +a2 = (1 - alpha_y).*alpha_x; +a3 = alpha_y .* (1 - alpha_x); +a4 = alpha_y .* alpha_x; + +ind_1 = px_0 * nr + py_0 + 1; +ind_2 = (px_0 + 1) * nr + py_0 + 1; +ind_3 = px_0 * nr + (py_0 + 1) + 1; +ind_4 = (px_0 + 1) * nr + (py_0 + 1) + 1; + +ind_new = (px(good_points)-1)*nr + py(good_points); + + +Irec(ind_new) = a1 .* I(ind_1) + a2 .* I(ind_2) + a3 .* I(ind_3) + a4 .* I(ind_4); + + + +return; + + +% Convert in indices: + +fact = 3; + +[XX,YY]= meshgrid(1:nc,1:nr); +[XXi,YYi]= meshgrid(1:1/fact:nc,1:1/fact:nr); + +%tic; +Iinterp = interp2(XX,YY,I,XXi,YYi); +%toc + +[nri,nci] = size(Iinterp); + + +ind_col = round(fact*(f(1)*xd(1,:)+c(1)))+1; +ind_row = round(fact*(f(2)*xd(2,:)+c(2)))+1; + +good_points = find((ind_col >=1)&(ind_col<=nci)&(ind_row >=1)& (ind_row <=nri)); diff --git a/src/g_calib/rectify_stereo_pair.m b/src/g_calib/rectify_stereo_pair.m new file mode 100755 index 0000000000000000000000000000000000000000..4fda19f68665feca3dec65da84c93b88cdf64f8a --- /dev/null +++ b/src/g_calib/rectify_stereo_pair.m @@ -0,0 +1,192 @@ +% rectify_stereo_pair.m +% +% Script file that rectifies a set of stereo images after stereo calibration +% This script loads the stereo calibration file Calib_Results_stereo generated by calib_stereo.m +% Therefore, type help calib_stereo for more information. + + +if ~exist('fc_right')|~exist('cc_right')|~exist('kc_right')|~exist('alpha_c_right')|~exist('fc_left')|~exist('cc_left')|~exist('kc_left')|~exist('alpha_c_left')|~exist('om')|~exist('T') + + if exist('Calib_Results_stereo.mat')~=2, + fprintf(1,'No stereo calibration data.\n'); + return; + else + fprintf(1,'\nLoading the stereo calibration file Calib_Results_stereo.mat.\n'); + load Calib_Results_stereo; % Load the stereo calibration result + end; + +end; + + + +fprintf(1,'\nCalculating the rotation to be applied to the right and left images in order to bring the epipolar lines aligned with the horizontal scan lines, and in correspondence...\n\n'); + +R = rodrigues(om); + +% Bring the 2 cameras in the same orientation by rotating them "minimally": +r_r = rodrigues(-om/2); +r_l = r_r'; +t = r_r * T; + +% Rotate both cameras so as to bring the translation vector in alignment with the (1;0;0) axis: +if abs(t(1)) > abs(t(2)), + type_stereo = 0; + uu = [1;0;0]; % Horizontal epipolar lines +else + type_stereo = 1; + uu = [0;1;0]; % Vertical epipolar lines +end; +if dot(uu,t)<0, + uu = -uu; % Swtich side of the vector +end; +ww = cross(t,uu); +ww = ww/norm(ww); +ww = acos(abs(dot(t,uu))/(norm(t)*norm(uu)))*ww; +R2 = rodrigues(ww); + + +% Global rotations to be applied to both views: +R_R = R2 * r_r; +R_L = R2 * r_l; + + +% The resulting rigid motion between the two cameras after image rotations (substitutes of om, R and T): +R_new = eye(3); +om_new = zeros(3,1); +T_new = R_R*T; + + + +% Computation of the *new* intrinsic parameters for both left and right cameras: + +% Vertical focal length *MUST* be the same for both images (here, we are trying to find a focal length that retains as much information contained in the original distorted images): +if kc_left(1) < 0, + fc_y_left_new = fc_left(2) * (1 + kc_left(1)*(nx^2 + ny^2)/(4*fc_left(2)^2)); +else + fc_y_left_new = fc_left(2); +end; +if kc_right(1) < 0, + fc_y_right_new = fc_right(2) * (1 + kc_right(1)*(nx^2 + ny^2)/(4*fc_right(2)^2)); +else + fc_y_right_new = fc_right(2); +end; +fc_y_new = min(fc_y_left_new,fc_y_right_new); + + +% For simplicity, let's pick the same value for the horizontal focal length as the vertical focal length (resulting into square pixels): +fc_left_new = round([fc_y_new;fc_y_new]); +fc_right_new = round([fc_y_new;fc_y_new]); + +% Select the new principal points to maximize the visible area in the rectified images + +cc_left_new = [(nx-1)/2;(ny-1)/2] - mean(project_points2([normalize_pixel([0 nx-1 nx-1 0; 0 0 ny-1 ny-1],fc_left,cc_left,kc_left,alpha_c_left);[1 1 1 1]],rodrigues(R_L),zeros(3,1),fc_left_new,[0;0],zeros(5,1),0),2); +cc_right_new = [(nx-1)/2;(ny-1)/2] - mean(project_points2([normalize_pixel([0 nx-1 nx-1 0; 0 0 ny-1 ny-1],fc_right,cc_right,kc_right,alpha_c_right);[1 1 1 1]],rodrigues(R_R),zeros(3,1),fc_right_new,[0;0],zeros(5,1),0),2); + + +% For simplivity, set the principal points for both cameras to be the average of the two principal points. +if ~type_stereo, + %-- Horizontal stereo + cc_y_new = (cc_left_new(2) + cc_right_new(2))/2; + cc_left_new = [cc_left_new(1);cc_y_new]; + cc_right_new = [cc_right_new(1);cc_y_new]; +else + %-- Vertical stereo + cc_x_new = (cc_left_new(1) + cc_right_new(1))/2; + cc_left_new = [cc_x_new;cc_left_new(2)]; + cc_right_new = [cc_x_new;cc_right_new(2)]; +end; + + +% Of course, we do not want any skew or distortion after rectification: +alpha_c_left_new = 0; +alpha_c_right_new = 0; +kc_left_new = zeros(5,1); +kc_right_new = zeros(5,1); + + +% The resulting left and right camera matrices: +KK_left_new = [fc_left_new(1) fc_left_new(1)*alpha_c_left_new cc_left_new(1);0 fc_left_new(2) cc_left_new(2); 0 0 1]; +KK_right_new = [fc_right_new(1) fc_right_new(1)*alpha_c_right cc_right_new(1);0 fc_right_new(2) cc_right_new(2); 0 0 1]; + +% The sizes of the images are the same: +nx_right_new = nx; +ny_right_new = ny; +nx_left_new = nx; +ny_left_new = ny; + +% Save the resulting extrinsic and intrinsic paramters into a file: +fprintf(1,'Saving the *NEW* set of intrinsic and extrinsic parameters corresponding to the images *AFTER* rectification under Calib_Results_stereo_rectified.mat...\n\n'); +save Calib_Results_stereo_rectified om_new R_new T_new fc_left_new cc_left_new kc_left_new alpha_c_left_new KK_left_new fc_right_new cc_right_new kc_right_new alpha_c_right_new KK_right_new nx_right_new ny_right_new nx_left_new ny_left_new + + + +% Let's rectify the entire set of calibration images: + +fprintf(1,'Pre-computing the necessary data to quickly rectify the images (may take a while depending on the image resolution, but needs to be done only once - even for color images)...\n\n'); + +% Pre-compute the necessary indices and blending coefficients to enable quick rectification: +[Irec_junk_left,ind_new_left,ind_1_left,ind_2_left,ind_3_left,ind_4_left,a1_left,a2_left,a3_left,a4_left] = rect_index(zeros(ny,nx),R_L,fc_left,cc_left,kc_left,alpha_c_left,KK_left_new); +[Irec_junk_left,ind_new_right,ind_1_right,ind_2_right,ind_3_right,ind_4_right,a1_right,a2_right,a3_right,a4_right] = rect_index(zeros(ny,nx),R_R,fc_right,cc_right,kc_right,alpha_c_right,KK_right_new); + +clear Irec_junk_left + +if 0, + %% Test of rectification for 2 images: + + % left image: + I = double(rgb2gray(imread('left.jpg'))); + I2 = 255*ones(ny,nx); + I2(ind_new_left) = uint8(a1_left .* I(ind_1_left) + a2_left .* I(ind_2_left) + a3_left .* I(ind_3_left) + a4_left .* I(ind_4_left)); + imwrite(uint8(I2),gray(256),'left_rect.jpg','jpg'); + + % right image: + I = double(rgb2gray(imread('right.jpg'))); + I2 = 255*ones(ny,nx); + I2(ind_new_right) = uint8(a1_right .* I(ind_1_right) + a2_right .* I(ind_2_right) + a3_right .* I(ind_3_right) + a4_right .* I(ind_4_right)); + imwrite(uint8(I2),gray(256),'right_rect.jpg','jpg'); + +end; + + + + +% Loop around all the frames now: (if there are images to rectify) + +if ~isempty(calib_name_left)&~isempty(calib_name_right), + + fprintf(1,'Rectifying all the images (this should be fast)...\n\n'); + + % Rectify all the images: (This is fastest way to proceed: precompute the set of image indices, and blending coefficients before actual image warping!) + + for kk = find(active_images); + + % Left image: + + I = load_image(kk,calib_name_left,format_image_left,type_numbering_left,image_numbers_left,N_slots_left); + + fprintf(1,'Image warping...\n'); + + I2 = 255*ones(ny,nx); + I2(ind_new_left) = uint8(a1_left .* I(ind_1_left) + a2_left .* I(ind_2_left) + a3_left .* I(ind_3_left) + a4_left .* I(ind_4_left)); + + write_image(I2,kk,[calib_name_left '_rectified'],format_image_left,type_numbering_left,image_numbers_left,N_slots_left ), + + fprintf(1,'\n'); + + % Right image: + + I = load_image(kk,calib_name_right,format_image_right,type_numbering_right,image_numbers_right,N_slots_right); + + fprintf(1,'Image warping...\n'); + + I2 = 255*ones(ny,nx); + I2(ind_new_right) = uint8(a1_right .* I(ind_1_right) + a2_right .* I(ind_2_right) + a3_right .* I(ind_3_right) + a4_right .* I(ind_4_right)); + + write_image(I2,kk,[calib_name_right '_rectified'],format_image_right,type_numbering_right,image_numbers_right,N_slots_right ); + + fprintf(1,'\n'); + + end; + +end; + diff --git a/src/g_calib/reproject_calib.m b/src/g_calib/reproject_calib.m new file mode 100755 index 0000000000000000000000000000000000000000..0eb7dc060668683f1caae4fedef146422936a71b --- /dev/null +++ b/src/g_calib/reproject_calib.m @@ -0,0 +1,216 @@ +%%%%%%%%%%%%%%%%%%%% REPROJECT ON THE IMAGES %%%%%%%%%%%%%%%%%%%%%%%% + +if ~exist('n_ima')|~exist('fc'), + fprintf(1,'No calibration data available.\n'); + return; +end; + +if ~exist('no_image'), + no_image = 0; +end; + +if ~exist('nx')&~exist('ny'), + fprintf(1,'WARNING: No image size (nx,ny) available. Setting nx=640 and ny=480\n'); + nx = 640; + ny = 480; +end; + + +check_active_images; + + +% Color code for each image: + +colors = 'brgkcm'; + +% Reproject the patterns on the images, and compute the pixel errors: + +% Reload the images if necessary +if n_ima ~= 0, +if ~exist(['omc_' num2str(ind_active(1)) ]), + fprintf(1,'Need to calibrate before showing image reprojection. Maybe need to load Calib_Results.mat file.\n'); + return; +end; +end; + +if n_ima ~= 0, +if ~no_image, + if ~exist(['I_' num2str(ind_active(1)) ]'), + n_ima_save = n_ima; + active_images_save = active_images; + ima_read_calib; + n_ima = n_ima_save; + active_images = active_images_save; + check_active_images; + if no_image_file, + fprintf(1,'WARNING: Do not show the original images\n'); %return; + end; + end; +else + no_image_file = 1; +end; +end; + + +if ~exist('dont_ask'), + dont_ask = 0; +end; + + +if (~dont_ask)&(length(ind_active)>1), + ima_numbers = input('Number(s) of image(s) to show ([] = all images) = '); +else + ima_numbers = []; +end; + + +if isempty(ima_numbers), + ima_proc = 1:n_ima; +else + ima_proc = ima_numbers; +end; + + +figure(5); +for kk = ima_proc, %1:n_ima, + if exist(['y_' num2str(kk)]), + if active_images(kk) & eval(['~isnan(y_' num2str(kk) '(1,1))']), + eval(['plot(ex_' num2str(kk) '(1,:),ex_' num2str(kk) '(2,:),''' colors(rem(kk-1,6)+1) '+'');']); + hold on; + end; + end; +end; +hold off; +axis('equal'); +title('Reprojection error (in pixel)'); +xlabel('x'); +ylabel('y'); +drawnow; +if n_ima==0, + text(.5,.5,'No image data available','fontsize',24,'horizontalalignment' ,'center'); +end; + + +set(5,'color',[1 1 1]); +set(5,'Name','error','NumberTitle','off'); + + +no_grid = 0; + +for kk = ima_proc, + if exist(['y_' num2str(kk)]), + if active_images(kk) & eval(['~isnan(y_' num2str(kk) '(1,1))']), + + if exist(['I_' num2str(kk)]), + eval(['I = I_' num2str(kk) ';']); + else + I = 255*ones(ny,nx); + end; + + + + figure(5+kk); + image(I); hold on; + colormap(gray(256)); + + + if ~no_grid, + + eval(['x_kk = x_' num2str(kk) ';']); + + N_kk = size(x_kk,2); + + if ~exist(['n_sq_x_' num2str(kk)])|~exist(['n_sq_y_' num2str(kk)]), + no_grid = 1; + end; + + if ~no_grid, + eval(['n_sq_x = n_sq_x_' num2str(kk) ';']); + eval(['n_sq_y = n_sq_y_' num2str(kk) ';']); + if (N_kk ~= ((n_sq_x+1)*(n_sq_y+1))), + no_grid = 1; + end; + end; + + end; + + if ~no_grid, + + % plot more things on the figure (to help the user): + + Nx = n_sq_x+1; + Ny = n_sq_y+1; + + ind_ori = (Ny - 1) * Nx + 1; + ind_X = Nx*Ny; + ind_Y = 1; + ind_XY = Nx; + + xo = x_kk(1,ind_ori); + yo = x_kk(2,ind_ori); + + xX = x_kk(1,ind_X); + yX = x_kk(2,ind_X); + + xY = x_kk(1,ind_Y); + yY = x_kk(2,ind_Y); + + xXY = x_kk(1,ind_XY); + yXY = x_kk(2,ind_XY); + + uu = cross(cross([xo;yo;1],[xXY;yXY;1]),cross([xX;yX;1],[xY;yY;1])); + xc = uu(1)/uu(3); + yc = uu(2)/uu(3); + + bbb = cross(cross([xo;yo;1],[xY;yY;1]),cross([xX;yX;1],[xXY;yXY;1])); + uu = cross(cross([xo;yo;1],[xX;yX;1]),cross([xc;yc;1],bbb)); + xXc = uu(1)/uu(3); + yXc = uu(2)/uu(3); + + bbb = cross(cross([xo;yo;1],[xX;yX;1]),cross([xY;yY;1],[xXY;yXY;1])); + uu = cross(cross([xo;yo;1],[xY;yY;1]),cross([xc;yc;1],bbb)); + xYc = uu(1)/uu(3); + yYc = uu(2)/uu(3); + + uX = [xXc - xc;yXc - yc]; + uY = [xYc - xc;yYc - yc]; + uO = [xo - xc;yo - yc]; + + uX = uX / norm(uX); + uY = uY / norm(uY); + uO = uO / norm(uO); + + delta = 30; + + plot([xo;xX]+1,[yo;yX]+1,'g-','linewidth',2); + plot([xo;xY]+1,[yo;yY]+1,'g-','linewidth',2); + text(xXc + delta * uX(1) +1 ,yXc + delta * uX(2)+1,'X','color','g','Fontsize',14); + text(xYc + delta * uY(1)+1 ,yYc + delta * uY(2)+1,'Y','color','g','Fontsize',14,'HorizontalAlignment','center'); + text(xo + delta * uO(1) +1,yo + delta * uO(2)+1,'O','color','g','Fontsize',14); + + end; + + + title(['Image ' num2str(kk) ' - Image points (+) and reprojected grid points (o)']); + eval(['plot(x_' num2str(kk) '(1,:)+1,x_' num2str(kk) '(2,:)+1,''r+'');']); + eval(['plot(y_' num2str(kk) '(1,:)+1,y_' num2str(kk) '(2,:)+1,''' colors(rem(kk-1,6)+1) 'o'');']); + eval(['quiver(y_' num2str(kk) '(1,:)+1,y_' num2str(kk) '(2,:)+1,ex_' num2str(kk) '(1,:),ex_' num2str(kk) '(2,:),1,''' colors(rem(kk-1,6)+1) ''');']); + zoom on; + axis([1 nx 1 ny]); + hold off; + drawnow; + + + set(5+kk,'color',[1 1 1]); + set(5+kk,'Name',num2str(kk),'NumberTitle','off'); + + + + end; + end; +end; + +if n_ima ~= 0, +err_std = std(ex')'; +fprintf(1,'Pixel error: err = [%3.5f %3.5f] (all active images)\n\n',err_std); +end; diff --git a/src/g_calib/reproject_calib_no_read.m b/src/g_calib/reproject_calib_no_read.m new file mode 100755 index 0000000000000000000000000000000000000000..7e1cfc6cd61d2d2bb6c82a58d3e36a16cdf5dbf8 --- /dev/null +++ b/src/g_calib/reproject_calib_no_read.m @@ -0,0 +1,248 @@ +%%%%%%%%%%%%%%%%%%%% REPROJECT ON THE IMAGES %%%%%%%%%%%%%%%%%%%%%%%% + +if ~exist('n_ima')|~exist('fc'), + fprintf(1,'No calibration data available.\n'); + return; +end; + +if ~exist('no_image'), + no_image = 0; +end; + +if ~exist('nx')&~exist('ny'), + fprintf(1,'WARNING: No image size (nx,ny) available. Setting nx=640 and ny=480\n'); + nx = 640; + ny = 480; +end; + + +check_active_images; + + +% Color code for each image: + +colors = 'brgkcm'; + +% Reproject the patterns on the images, and compute the pixel errors: + +% Reload the images if necessary +if n_ima ~= 0, +if ~exist(['omc_' num2str(ind_active(1)) ]), + fprintf(1,'Need to calibrate before showing image reprojection. Maybe need to load Calib_Results.mat file.\n'); + return; +end; +end; + +%if ~no_image, +% if ~exist(['I_' num2str(ind_active(1)) ]'), +% n_ima_save = n_ima; +% active_images_save = active_images; +% ima_read_calib; +% n_ima = n_ima_save; +% active_images = active_images_save; +% check_active_images; +% if no_image_file, +% fprintf(1,'WARNING: Do not show the original images\n'); %return; +% end; +% end; +%else +% no_image_file = 1; +%end; + + +if ~exist('dont_ask'), + dont_ask = 0; +end; + + +if (~dont_ask)&(length(ind_active)>1), + ima_numbers = input('Number(s) of image(s) to show ([] = all images) = '); +else + ima_numbers = []; +end; + + +if isempty(ima_numbers), + ima_proc = 1:n_ima; +else + ima_proc = ima_numbers; +end; + + +figure(5); +for kk = ima_proc, %1:n_ima, + if exist(['y_' num2str(kk)]), + if active_images(kk) & eval(['~isnan(y_' num2str(kk) '(1,1))']), + eval(['plot(ex_' num2str(kk) '(1,:),ex_' num2str(kk) '(2,:),''' colors(rem(kk-1,6)+1) '+'');']); + hold on; + end; + end; +end; +hold off; +axis('equal'); +title('Reprojection error (in pixel)'); +xlabel('x'); +ylabel('y'); +drawnow; +if n_ima==0, + text(.5,.5,'No image data available','fontsize',24,'horizontalalignment' ,'center'); +end; + + +set(5,'color',[1 1 1]); +set(5,'Name','error','NumberTitle','off'); + + +no_grid = 0; + +for kk = ima_proc, + if exist(['y_' num2str(kk)]), + if active_images(kk) & eval(['~isnan(y_' num2str(kk) '(1,1))']), + + + if ~type_numbering, + number_ext = num2str(image_numbers(kk)); + else + number_ext = sprintf(['%.' num2str(N_slots) 'd'],image_numbers(kk)); + end; + + ima_name = [calib_name number_ext '.' format_image]; + + + if ~exist(ima_name), + + I = 255*ones(ny,nx); + + else + + fprintf(1,'Loading image %s...\n',ima_name); + + if format_image(1) == 'p', + if format_image(2) == 'p', + I = double(loadppm(ima_name)); + else + I = double(loadpgm(ima_name)); + end; + else + if format_image(1) == 'r', + I = readras(ima_name); + else + I = double(imread(ima_name)); + end; + end; + + + if size(I,3)>1, + I = 0.299 * I(:,:,1) + 0.5870 * I(:,:,2) + 0.114 * I(:,:,3); + end; + + [ny,nx,junk] = size(I); + + end; + + + + + + figure(5+kk); + image(I); hold on; + colormap(gray(256)); + + + if ~no_grid, + + eval(['x_kk = x_' num2str(kk) ';']); + + N_kk = size(x_kk,2); + + if ~exist(['n_sq_x_' num2str(kk)])|~exist(['n_sq_y_' num2str(kk)]), + no_grid = 1; + end; + + if ~no_grid, + eval(['n_sq_x = n_sq_x_' num2str(kk) ';']); + eval(['n_sq_y = n_sq_y_' num2str(kk) ';']); + if (N_kk ~= ((n_sq_x+1)*(n_sq_y+1))), + no_grid = 1; + end; + end; + + end; + + if ~no_grid, + + % plot more things on the figure (to help the user): + + Nx = n_sq_x+1; + Ny = n_sq_y+1; + + ind_ori = (Ny - 1) * Nx + 1; + ind_X = Nx*Ny; + ind_Y = 1; + ind_XY = Nx; + + xo = x_kk(1,ind_ori); + yo = x_kk(2,ind_ori); + + xX = x_kk(1,ind_X); + yX = x_kk(2,ind_X); + + xY = x_kk(1,ind_Y); + yY = x_kk(2,ind_Y); + + xXY = x_kk(1,ind_XY); + yXY = x_kk(2,ind_XY); + + uu = cross(cross([xo;yo;1],[xXY;yXY;1]),cross([xX;yX;1],[xY;yY;1])); + xc = uu(1)/uu(3); + yc = uu(2)/uu(3); + + bbb = cross(cross([xo;yo;1],[xY;yY;1]),cross([xX;yX;1],[xXY;yXY;1])); + uu = cross(cross([xo;yo;1],[xX;yX;1]),cross([xc;yc;1],bbb)); + xXc = uu(1)/uu(3); + yXc = uu(2)/uu(3); + + bbb = cross(cross([xo;yo;1],[xX;yX;1]),cross([xY;yY;1],[xXY;yXY;1])); + uu = cross(cross([xo;yo;1],[xY;yY;1]),cross([xc;yc;1],bbb)); + xYc = uu(1)/uu(3); + yYc = uu(2)/uu(3); + + uX = [xXc - xc;yXc - yc]; + uY = [xYc - xc;yYc - yc]; + uO = [xo - xc;yo - yc]; + + uX = uX / norm(uX); + uY = uY / norm(uY); + uO = uO / norm(uO); + + delta = 30; + + plot([xo;xX]+1,[yo;yX]+1,'g-','linewidth',2); + plot([xo;xY]+1,[yo;yY]+1,'g-','linewidth',2); + text(xXc + delta * uX(1) +1 ,yXc + delta * uX(2)+1,'X','color','g','Fontsize',14); + text(xYc + delta * uY(1)+1 ,yYc + delta * uY(2)+1,'Y','color','g','Fontsize',14,'HorizontalAlignment','center'); + text(xo + delta * uO(1) +1,yo + delta * uO(2)+1,'O','color','g','Fontsize',14); + + end; + + + title(['Image ' num2str(kk) ' - Image points (+) and reprojected grid points (o)']); + eval(['plot(x_' num2str(kk) '(1,:)+1,x_' num2str(kk) '(2,:)+1,''r+'');']); + eval(['plot(y_' num2str(kk) '(1,:)+1,y_' num2str(kk) '(2,:)+1,''' colors(rem(kk-1,6)+1) 'o'');']); + zoom on; + axis([1 nx 1 ny]); + hold off; + drawnow; + + + set(5+kk,'color',[1 1 1]); + set(5+kk,'Name',num2str(kk),'NumberTitle','off'); + + end; + end; +end; + +if n_ima ~= 0, +err_std = std(ex')'; +fprintf(1,'Pixel error: err = [%3.5f %3.5f] (all active images)\n\n',err_std); +end; diff --git a/src/g_calib/rigid_motion.m b/src/g_calib/rigid_motion.m new file mode 100755 index 0000000000000000000000000000000000000000..a4a5527aca25fa3a3cf35937a7ac6fd6351a0df8 --- /dev/null +++ b/src/g_calib/rigid_motion.m @@ -0,0 +1,66 @@ +function [Y,dYdom,dYdT] = rigid_motion(X,om,T); + +%rigid_motion.m +% +%[Y,dYdom,dYdT] = rigid_motion(X,om,T) +% +%Computes the rigid motion transformation Y = R*X+T, where R = rodrigues(om). +% +%INPUT: X: 3D structure in the world coordinate frame (3xN matrix for N points) +% (om,T): Rigid motion parameters between world coordinate frame and camera reference frame +% om: rotation vector (3x1 vector); T: translation vector (3x1 vector) +% +%OUTPUT: Y: 3D coordinates of the structure points in the camera reference frame (3xN matrix for N points) +% dYdom: Derivative of Y with respect to om ((3N)x3 matrix) +% dYdT: Derivative of Y with respect to T ((3N)x3 matrix) +% +%Definitions: +%Let P be a point in 3D of coordinates X in the world reference frame (stored in the matrix X) +%The coordinate vector of P in the camera reference frame is: Y = R*X + T +%where R is the rotation matrix corresponding to the rotation vector om: R = rodrigues(om); +% +%Important function called within that program: +% +%rodrigues.m: Computes the rotation matrix corresponding to a rotation vector + + + +if nargin < 3, + T = zeros(3,1); + if nargin < 2, + om = zeros(3,1); + if nargin < 1, + error('Need at least a 3D structure as input (in rigid_motion.m)'); + return; + end; + end; +end; + + +[R,dRdom] = rodrigues(om); + +[m,n] = size(X); + +Y = R*X + repmat(T,[1 n]); + +if nargout > 1, + + +dYdR = zeros(3*n,9); +dYdT = zeros(3*n,3); + +dYdR(1:3:end,1:3:end) = X'; +dYdR(2:3:end,2:3:end) = X'; +dYdR(3:3:end,3:3:end) = X'; + +dYdT(1:3:end,1) = ones(n,1); +dYdT(2:3:end,2) = ones(n,1); +dYdT(3:3:end,3) = ones(n,1); + +dYdom = dYdR * dRdom; + +end; + + + + diff --git a/src/g_calib/rodrigues.m b/src/g_calib/rodrigues.m new file mode 100755 index 0000000000000000000000000000000000000000..088aa4e9680a0b2546ae9fe6bece9e49239a8f1f --- /dev/null +++ b/src/g_calib/rodrigues.m @@ -0,0 +1,267 @@ +function [out,dout]=rodrigues(in) + +% RODRIGUES Transform rotation matrix into rotation vector and viceversa. +% +% Sintax: [OUT]=RODRIGUES(IN) +% If IN is a 3x3 rotation matrix then OUT is the +% corresponding 3x1 rotation vector +% if IN is a rotation 3-vector then OUT is the +% corresponding 3x3 rotation matrix +% + +%% +%% Copyright (c) March 1993 -- Pietro Perona +%% California Institute of Technology +%% + +%% ALL CHECKED BY JEAN-YVES BOUGUET, October 1995. +%% FOR ALL JACOBIAN MATRICES !!! LOOK AT THE TEST AT THE END !! + +%% BUG when norm(om)=pi fixed -- April 6th, 1997; +%% Jean-Yves Bouguet + +%% Add projection of the 3x3 matrix onto the set of special ortogonal matrices SO(3) by SVD -- February 7th, 2003; +%% Jean-Yves Bouguet + +% BUG FOR THE CASE norm(om)=pi fixed by Mike Burl on Feb 27, 2007 + + +[m,n] = size(in); +%bigeps = 10e+4*eps; +bigeps = 10e+20*eps; + +if ((m==1) & (n==3)) | ((m==3) & (n==1)) %% it is a rotation vector + theta = norm(in); + if theta < eps + R = eye(3); + + %if nargout > 1, + + dRdin = [0 0 0; + 0 0 1; + 0 -1 0; + 0 0 -1; + 0 0 0; + 1 0 0; + 0 1 0; + -1 0 0; + 0 0 0]; + + %end; + + else + if n==length(in) in=in'; end; %% make it a column vec. if necess. + + %m3 = [in,theta] + + dm3din = [eye(3);in'/theta]; + + omega = in/theta; + + %m2 = [omega;theta] + + dm2dm3 = [eye(3)/theta -in/theta^2; zeros(1,3) 1]; + + alpha = cos(theta); + beta = sin(theta); + gamma = 1-cos(theta); + omegav=[[0 -omega(3) omega(2)];[omega(3) 0 -omega(1)];[-omega(2) omega(1) 0 ]]; + A = omega*omega'; + + %m1 = [alpha;beta;gamma;omegav;A]; + + dm1dm2 = zeros(21,4); + dm1dm2(1,4) = -sin(theta); + dm1dm2(2,4) = cos(theta); + dm1dm2(3,4) = sin(theta); + dm1dm2(4:12,1:3) = [0 0 0 0 0 1 0 -1 0; + 0 0 -1 0 0 0 1 0 0; + 0 1 0 -1 0 0 0 0 0]'; + + w1 = omega(1); + w2 = omega(2); + w3 = omega(3); + + dm1dm2(13:21,1) = [2*w1;w2;w3;w2;0;0;w3;0;0]; + dm1dm2(13: 21,2) = [0;w1;0;w1;2*w2;w3;0;w3;0]; + dm1dm2(13:21,3) = [0;0;w1;0;0;w2;w1;w2;2*w3]; + + R = eye(3)*alpha + omegav*beta + A*gamma; + + dRdm1 = zeros(9,21); + + dRdm1([1 5 9],1) = ones(3,1); + dRdm1(:,2) = omegav(:); + dRdm1(:,4:12) = beta*eye(9); + dRdm1(:,3) = A(:); + dRdm1(:,13:21) = gamma*eye(9); + + dRdin = dRdm1 * dm1dm2 * dm2dm3 * dm3din; + + + end; + out = R; + dout = dRdin; + + %% it is prob. a rot matr. +elseif ((m==n) & (m==3) & (norm(in' * in - eye(3)) < bigeps)... + & (abs(det(in)-1) < bigeps)) + R = in; + + % project the rotation matrix to SO(3); + [U,S,V] = svd(R); + R = U*V'; + + tr = (trace(R)-1)/2; + dtrdR = [1 0 0 0 1 0 0 0 1]/2; + theta = real(acos(tr)); + + + if sin(theta) >= 1e-4, + + dthetadtr = -1/sqrt(1-tr^2); + + dthetadR = dthetadtr * dtrdR; + % var1 = [vth;theta]; + vth = 1/(2*sin(theta)); + dvthdtheta = -vth*cos(theta)/sin(theta); + dvar1dtheta = [dvthdtheta;1]; + + dvar1dR = dvar1dtheta * dthetadR; + + + om1 = [R(3,2)-R(2,3), R(1,3)-R(3,1), R(2,1)-R(1,2)]'; + + dom1dR = [0 0 0 0 0 1 0 -1 0; + 0 0 -1 0 0 0 1 0 0; + 0 1 0 -1 0 0 0 0 0]; + + % var = [om1;vth;theta]; + dvardR = [dom1dR;dvar1dR]; + + % var2 = [om;theta]; + om = vth*om1; + domdvar = [vth*eye(3) om1 zeros(3,1)]; + dthetadvar = [0 0 0 0 1]; + dvar2dvar = [domdvar;dthetadvar]; + + + out = om*theta; + domegadvar2 = [theta*eye(3) om]; + + dout = domegadvar2 * dvar2dvar * dvardR; + + + else + if tr > 0; % case norm(om)=0; + + out = [0 0 0]'; + + dout = [0 0 0 0 0 1/2 0 -1/2 0; + 0 0 -1/2 0 0 0 1/2 0 0; + 0 1/2 0 -1/2 0 0 0 0 0]; + else + + % case norm(om)=pi; + if(0) + + %% fixed April 6th by Bouguet -- not working in all cases! + out = theta * (sqrt((diag(R)+1)/2).*[1;2*(R(1,2:3)>=0)'-1]); + %keyboard; + + else + + % Solution by Mike Burl on Feb 27, 2007 + % This is a better way to determine the signs of the + % entries of the rotation vector using a hash table on all + % the combinations of signs of a pairs of products (in the + % rotation matrix) + + % Define hashvec and Smat + hashvec = [0; -1; -3; -9; 9; 3; 1; 13; 5; -7; -11]; + Smat = [1,1,1; 1,0,-1; 0,1,-1; 1,-1,0; 1,1,0; 0,1,1; 1,0,1; 1,1,1; 1,1,-1; + 1,-1,-1; 1,-1,1]; + + M = (R+eye(3,3))/2; + uabs = sqrt(M(1,1)); + vabs = sqrt(M(2,2)); + wabs = sqrt(M(3,3)); + + mvec = [M(1,2), M(2,3), M(1,3)]; + syn = ((mvec > 1e-4) - (mvec < -1e-4)); % robust sign() function + hash = syn * [9; 3; 1]; + idx = find(hash == hashvec); + svec = Smat(idx,:)'; + + out = theta * [uabs; vabs; wabs] .* svec; + + end; + + if nargout > 1, + fprintf(1,'WARNING!!!! Jacobian domdR undefined!!!\n'); + dout = NaN*ones(3,9); + end; + end; + end; + +else + error('Neither a rotation matrix nor a rotation vector were provided'); +end; + +return; + +%% test of the Jacobians: + +%%%% TEST OF dRdom: +om = randn(3,1); +dom = randn(3,1)/1000000; + +[R1,dR1] = rodrigues(om); +R2 = rodrigues(om+dom); + +R2a = R1 + reshape(dR1 * dom,3,3); + +gain = norm(R2 - R1)/norm(R2 - R2a) + +%%% TEST OF dOmdR: +om = randn(3,1); +R = rodrigues(om); +dom = randn(3,1)/10000; +dR = rodrigues(om+dom) - R; + +[omc,domdR] = rodrigues(R); +[om2] = rodrigues(R+dR); + +om_app = omc + domdR*dR(:); + +gain = norm(om2 - omc)/norm(om2 - om_app) + + +%%% OTHER BUG: (FIXED NOW!!!) + +omu = randn(3,1); +omu = omu/norm(omu) +om = pi*omu; +[R,dR]= rodrigues(om); +[om2] = rodrigues(R); +[om om2] + +%%% NORMAL OPERATION + +om = randn(3,1); +[R,dR]= rodrigues(om); +[om2] = rodrigues(R); +[om om2] + +return + +% Test: norm(om) = pi + +u = randn(3,1); +u = u / sqrt(sum(u.^2)); +om = pi*u; +R = rodrigues(om); + +R2 = rodrigues(rodrigues(R)); + +norm(R - R2) \ No newline at end of file diff --git a/src/g_calib/rotation.m b/src/g_calib/rotation.m new file mode 100755 index 0000000000000000000000000000000000000000..87ee2fe0f08e78d544738d44ccfa87f93d1fb957 --- /dev/null +++ b/src/g_calib/rotation.m @@ -0,0 +1,23 @@ +function [] = rotation(st); + +if nargin < 1, + st= 1; +end; + + +fig = gcf; + +ax = gca; + +vv = get(ax,'view'); + +az = vv(1); +el = vv(2); + +for azi = az:-abs(st):az-360, + + set(ax,'view',[azi el]); + figure(fig); + drawnow; + +end; diff --git a/src/g_calib/run_error_analysis.m b/src/g_calib/run_error_analysis.m new file mode 100755 index 0000000000000000000000000000000000000000..095e17e3af40d28a15deb5cc8a481067f7203292 --- /dev/null +++ b/src/g_calib/run_error_analysis.m @@ -0,0 +1,65 @@ +%%% Program that launchs the complete + +for N_ima_active = 1:30, + + error_analysis; + +end; + + + +return; + + +f = []; +f_std = []; + +c = []; +c_std = []; + +k = []; +k_std = []; + +NN = 30; + +for rr = 1:NN, + + load(['Calib_Accuracies_' num2str(rr)]); + + [m1,s1] = mean_std_robust(fc_list(1,:)); + [m2,s2] = mean_std_robust(fc_list(2,:)); + + f = [f [m1;m2]]; + f_std = [f_std [s1;s2]]; + + [m1,s1] = mean_std_robust(cc_list(1,:)); + [m2,s2] = mean_std_robust(cc_list(2,:)); + + c = [c [m1;m2]]; + c_std = [c_std [s1;s2]]; + + [m1,s1] = mean_std_robust(kc_list(1,:)); + [m2,s2] = mean_std_robust(kc_list(2,:)); + [m3,s3] = mean_std_robust(kc_list(3,:)); + [m4,s4] = mean_std_robust(kc_list(4,:)); + + k = [k [m1;m2;m3;m4]]; + k_std = [k_std [s1;s2;s3;s4]]; + +end; + +figure(1); +errorbar([1:NN;1:NN]',f',f_std'); +figure(2); +errorbar([1:NN;1:NN]',c',c_std'); +figure(3); +errorbar([1:NN;1:NN;1:NN;1:NN]',k',k_std'); + +figure(4); +semilogy(f_std'); + +figure(5); +semilogy(c_std'); + +figure(6); +semilogy(k_std'); diff --git a/src/g_calib/saveinr.m b/src/g_calib/saveinr.m new file mode 100755 index 0000000000000000000000000000000000000000..a176e39951d8a6efb795aef9dae5275e9b2c12d1 --- /dev/null +++ b/src/g_calib/saveinr.m @@ -0,0 +1,46 @@ +%SAVEINR Write an INRIMAGE format file +% +% SAVEINR(filename, im) +% +% Saves the specified image array in a INRIA image format file. +% +% SEE ALSO: loadinr +% +% Copyright (c) Peter Corke, 1999 Machine Vision Toolbox for Matlab + +% Peter Corke 1996 + +function saveinr(fname, im) + + fid = fopen(fname, 'w'); + [r,c] = size(im'); + + % build the header + hdr = []; + s = sprintf('#INRIMAGE-4#{\n'); + hdr = [hdr s]; + s = sprintf('XDIM=%d\n',c); + hdr = [hdr s]; + s = sprintf('YDIM=%d\n',r); + hdr = [hdr s]; + s = sprintf('ZDIM=1\n'); + hdr = [hdr s]; + s = sprintf('VDIM=1\n'); + hdr = [hdr s]; + s = sprintf('TYPE=float\n'); + hdr = [hdr s]; + s = sprintf('PIXSIZE=32\n'); + hdr = [hdr s]; + s = sprintf('SCALE=2**0\n'); + hdr = [hdr s]; + s = sprintf('CPU=sun\n#'); + hdr = [hdr s]; + + % make it 256 bytes long and write it + hdr256 = zeros(1,256); + hdr256(1:length(hdr)) = hdr; + fwrite(fid, hdr256, 'uchar'); + + % now the binary data + fwrite(fid, im', 'float32'); + fclose(fid) diff --git a/src/g_calib/savepgm.m b/src/g_calib/savepgm.m new file mode 100755 index 0000000000000000000000000000000000000000..0cee75de9fb5dd75858cb3a49bc265de4afef98d --- /dev/null +++ b/src/g_calib/savepgm.m @@ -0,0 +1,22 @@ +%SAVEPGM Write a PGM format file +% +% SAVEPGM(filename, im) +% +% Saves the specified image array in a binary (P5) format PGM image file. +% +% SEE ALSO: loadpgm +% +% Copyright (c) Peter Corke, 1999 Machine Vision Toolbox for Matlab + + +% Peter Corke 1994 + +function savepgm(fname, im) + + fid = fopen(fname, 'w'); + [r,c] = size(im'); + fprintf(fid, 'P5\n'); + fprintf(fid, '%d %d\n', r, c); + fprintf(fid, '255\n'); + fwrite(fid, im', 'uchar'); + fclose(fid); diff --git a/src/g_calib/saveppm.m b/src/g_calib/saveppm.m new file mode 100755 index 0000000000000000000000000000000000000000..ece092bf77569a692ff9daaa6d13009b2d6d52d7 --- /dev/null +++ b/src/g_calib/saveppm.m @@ -0,0 +1,45 @@ +%SAVEPPM Write a PPM format file +% +% SAVEPPM(filename, I) +% +% Saves the specified red, green and blue planes in a binary (P6) +% format PPM image file. +% +% SEE ALSO: loadppm +% +% Copyright (c) Peter Corke, 1999 Machine Vision Toolbox for Matlab + + +% Peter Corke 1994 + +function saveppm(fname, I) + +I = double(I); + +if size(I,3) == 1, + R = I; + G = I; + B = I; +else + R = I(:,:,1); + G = I(:,:,2); + B = I(:,:,3); +end; + +%keyboard; + + fid = fopen(fname, 'w'); + [r,c] = size(R'); + fprintf(fid, 'P6\n'); + fprintf(fid, '%d %d\n', r, c); + fprintf(fid, '255\n'); + R = R'; + G = G'; + B = B'; + im = [R(:) G(:) B(:)]; + %im = reshape(im,r,c*3); + im = im'; + %im = im(:); + fwrite(fid, im, 'uchar'); + fclose(fid); + diff --git a/src/g_calib/saving_calib.m b/src/g_calib/saving_calib.m new file mode 100755 index 0000000000000000000000000000000000000000..6f9b6c2a77015ea4db139f02c635efdd8134832e --- /dev/null +++ b/src/g_calib/saving_calib.m @@ -0,0 +1,220 @@ + +if ~exist('n_ima')|~exist('fc'), + fprintf(1,'No calibration data available.\n'); + return; +end; + +if ~exist('no_image'), + no_image = 0; +end; + +if ~exist('est_alpha'), + est_alpha = 0; +end; + +if ~exist('center_optim'), + center_optim = 1; +end; + +if ~exist('est_aspect_ratio'), + est_aspect_ratio = 1; +end; + +if ~exist('est_fc'); + est_fc = [1;1]; % Set to zero if you do not want to estimate the focal length (it may be useful! believe it or not!) +end; + +if exist('est_dist'), + if length(est_dist) == 4, + est_dist = [est_dist ; 0]; + end; +end; + +if exist('kc'), + if length(kc) == 4; + kc = [kc;0]; + end; +end; + +if ~exist('fc_error'), + fc_error = NaN*ones(2,1); +end; + +if ~exist('kc_error'), + kc_error = NaN*ones(5,1); +end; + +if ~exist('cc_error'), + cc_error = NaN*ones(2,1); +end; + +if ~exist('alpha_c_error'), + alpha_c_error = NaN; +end; + +check_active_images; + +if ~exist('solution_init'), solution_init = []; end; + +for kk = 1:n_ima, + if ~exist(['dX_' num2str(kk)]), eval(['dX_' num2str(kk) '= dX;']); end; + if ~exist(['dY_' num2str(kk)]), eval(['dY_' num2str(kk) '= dY;']); end; +end; + +if ~exist('solution'), + solution = []; +end; + +if ~exist('param_list'), + param_list = solution; +end; + +if ~exist('wintx'), + wintx = []; + winty = []; +end; + +if ~exist('dX_default'), + dX_default = []; + dY_default = []; +end; + +if ~exist('dX'), + dX = []; +end; +if ~exist('dY'), + dY = dX; +end; + +if ~exist('Hcal'), + Hcal = []; +end; +if ~exist('Wcal'), + Wcal = []; +end; +if ~exist('map'), + map = []; +end; + +if ~exist('wintx_default')|~exist('winty_default'), + wintx_default = max(round(nx/128),round(ny/96)); + winty_default = wintx_default; +end; + +if ~exist('alpha_c'), + alpha_c = 0; +end; + +if ~exist('err_std'), + err_std = [NaN;NaN]; +end; + +for kk = 1:n_ima, + if ~exist(['y_' num2str(kk)]), + eval(['y_' num2str(kk) ' = NaN*ones(2,1);']); + end; + if ~exist(['n_sq_x_' num2str(kk)]), + eval(['n_sq_x_' num2str(kk) ' = NaN;']); + eval(['n_sq_y_' num2str(kk) ' = NaN;']); + end; + if ~exist(['wintx_' num2str(kk)]), + eval(['wintx_' num2str(kk) ' = NaN;']); + eval(['winty_' num2str(kk) ' = NaN;']); + end; + if ~exist(['Tc_' num2str(kk)]), + eval(['Tc_' num2str(kk) ' = [NaN;NaN;NaN];']); + end; + if ~exist(['omc_' num2str(kk)]), + eval(['omc_' num2str(kk) ' = [NaN;NaN;NaN];']); + eval(['Rc_' num2str(kk) ' = NaN * ones(3,3);']); + end; + if ~exist(['omc_error_' num2str(kk)]), + eval(['omc_error_' num2str(kk) ' = NaN*ones(3,1);']); + end; + if ~exist(['Tc_error_' num2str(kk)]), + eval(['Tc_error_' num2str(kk) ' = NaN*ones(3,1);']); + end; + if ~exist(['Rc_' num2str(kk)]), + eval(['Rc_' num2str(kk) ' = rodrigues(omc_' num2str(kk) ');']); + end; +end; + +if ~exist('small_calib_image'), + small_calib_image = 0; +end; + +if ~exist('check_cond'), + check_cond = 1; +end; + +if ~exist('MaxIter'), + MaxIter = 30; +end; + +save_name = 'Calib_Results'; + +if exist([ save_name '.mat'])==2, + disp('WARNING: File Calib_Results.mat already exists'); + if exist('copyfile'), + pfn = -1; + cont = 1; + while cont, + pfn = pfn + 1; + postfix = ['_old' num2str(pfn)]; + save_name = [ 'Calib_Results' postfix]; + cont = (exist([ save_name '.mat'])==2); + end; + copyfile('Calib_Results.mat',[save_name '.mat']); + disp(['Copying the current Calib_Results.mat file to ' save_name '.mat']); + if exist('Calib_Results.m')==2, + copyfile('Calib_Results.m',[save_name '.m']); + disp(['Copying the current Calib_Results.m file to ' save_name '.m']); + end; + cont_save = 1; + else + disp('The file Calib_Results.mat is about to be changed.'); + cont_save = input('Do you want to continue? ([]=no,other=yes) ','s'); + cont_save = ~isempty(cont_save); + end; +else + cont_save = 1; +end; + + +if cont_save, + + save_name = 'Calib_Results'; + + if exist('calib_name'), + + fprintf(1,['\nSaving calibration results under ' save_name '.mat\n']); + + string_save = ['save ' save_name ' center_optim param_list active_images ind_active est_alpha est_dist est_aspect_ratio est_fc fc kc cc alpha_c fc_error kc_error cc_error alpha_c_error err_std ex x y solution solution_init wintx winty n_ima type_numbering N_slots small_calib_image first_num image_numbers format_image calib_name Hcal Wcal nx ny map dX_default dY_default KK inv_KK dX dY wintx_default winty_default no_image check_cond MaxIter']; + + for kk = 1:n_ima, + string_save = [string_save ' X_' num2str(kk) ' x_' num2str(kk) ' y_' num2str(kk) ' ex_' num2str(kk) ' omc_' num2str(kk) ' Rc_' num2str(kk) ' Tc_' num2str(kk) ' omc_error_' num2str(kk) ' Tc_error_' num2str(kk) ' H_' num2str(kk) ' n_sq_x_' num2str(kk) ' n_sq_y_' num2str(kk) ' wintx_' num2str(kk) ' winty_' num2str(kk) ' dX_' num2str(kk) ' dY_' num2str(kk)]; + end; + + else + + fprintf(1,['\nSaving calibration results under ' save_name '.mat (no image version)\n']); + + string_save = ['save ' save_name ' center_optim param_list active_images ind_active est_alpha est_dist est_aspect_ratio est_fc fc kc cc alpha_c fc_error kc_error cc_error alpha_c_error err_std ex x y solution solution_init wintx winty n_ima nx ny dX_default dY_default KK inv_KK dX dY wintx_default winty_default no_image check_cond MaxIter']; + + for kk = 1:n_ima, + string_save = [string_save ' X_' num2str(kk) ' x_' num2str(kk) ' y_' num2str(kk) ' ex_' num2str(kk) ' omc_' num2str(kk) ' Rc_' num2str(kk) ' Tc_' num2str(kk) ' omc_error_' num2str(kk) ' Tc_error_' num2str(kk) ' H_' num2str(kk) ' n_sq_x_' num2str(kk) ' n_sq_y_' num2str(kk) ' wintx_' num2str(kk) ' winty_' num2str(kk) ' dX_' num2str(kk) ' dY_' num2str(kk)]; + end; + + end; + + + + %fprintf(1,'To load later click on Load\n'); + + eval(string_save); + + saving_calib_ascii; + + fprintf(1,'done\n'); + +end; diff --git a/src/g_calib/saving_calib_ascii.m b/src/g_calib/saving_calib_ascii.m new file mode 100755 index 0000000000000000000000000000000000000000..147808292603687b16a0fa3961de3b94d4d0ef53 --- /dev/null +++ b/src/g_calib/saving_calib_ascii.m @@ -0,0 +1,74 @@ +if ~exist('save_name'), + save_name = 'Calib_Results'; +end; + +fprintf(1,'Generating the matlab script file %s.m containing the intrinsic and extrinsic parameters...\n',save_name) + + +fid = fopen([ save_name '.m'],'wt'); + +fprintf(fid,'%% Intrinsic and Extrinsic Camera Parameters\n'); +fprintf(fid,'%%\n'); +fprintf(fid,'%% This script file can be directly excecuted under Matlab to recover the camera intrinsic and extrinsic parameters.\n'); +fprintf(fid,'%% IMPORTANT: This file contains neither the structure of the calibration objects nor the image coordinates of the calibration points.\n'); +fprintf(fid,'%% All those complementary variables are saved in the complete matlab data file Calib_Results.mat.\n'); +fprintf(fid,'%% For more information regarding the calibration model visit http://www.vision.caltech.edu/bouguetj/calib_doc/\n'); +fprintf(fid,'\n\n'); +fprintf(fid,'%%-- Focal length:\n'); +fprintf(fid,'fc = [ %5.15f ; %5.15f ];\n',fc); +fprintf(fid,'\n'); +fprintf(fid,'%%-- Principal point:\n'); +fprintf(fid,'cc = [ %5.15f ; %5.15f ];\n',cc); +fprintf(fid,'\n'); +fprintf(fid,'%%-- Skew coefficient:\n'); +fprintf(fid,'alpha_c = %5.15f;\n',alpha_c); +fprintf(fid,'\n'); +fprintf(fid,'%%-- Distortion coefficients:\n'); +fprintf(fid,'kc = [ %5.15f ; %5.15f ; %5.15f ; %5.15f ; %5.15f ];\n',kc); +fprintf(fid,'\n'); +fprintf(fid,'%%-- Focal length uncertainty:\n'); +fprintf(fid,'fc_error = [ %5.15f ; %5.15f ];\n',fc_error); +fprintf(fid,'\n'); +fprintf(fid,'%%-- Principal point uncertainty:\n'); +fprintf(fid,'cc_error = [ %5.15f ; %5.15f ];\n',cc_error); +fprintf(fid,'\n'); +fprintf(fid,'%%-- Skew coefficient uncertainty:\n'); +fprintf(fid,'alpha_c_error = %5.15f;\n',alpha_c_error); +fprintf(fid,'\n'); +fprintf(fid,'%%-- Distortion coefficients uncertainty:\n'); +fprintf(fid,'kc_error = [ %5.15f ; %5.15f ; %5.15f ; %5.15f ; %5.15f ];\n',kc_error); +fprintf(fid,'\n'); +fprintf(fid,'%%-- Image size:\n'); +fprintf(fid,'nx = %d;\n',nx); +fprintf(fid,'ny = %d;\n',ny); +fprintf(fid,'\n'); +fprintf(fid,'\n'); +fprintf(fid,'%%-- Various other variables (may be ignored if you do not use the Matlab Calibration Toolbox):\n'); +fprintf(fid,'%%-- Those variables are used to control which intrinsic parameters should be optimized\n'); +fprintf(fid,'\n'); +fprintf(fid,'n_ima = %d;\t\t\t\t\t\t%% Number of calibration images\n',n_ima); +fprintf(fid,'est_fc = [ %d ; %d ];\t\t\t\t\t%% Estimation indicator of the two focal variables\n',est_fc); +fprintf(fid,'est_aspect_ratio = %d;\t\t\t\t%% Estimation indicator of the aspect ratio fc(2)/fc(1)\n',est_aspect_ratio); +fprintf(fid,'center_optim = %d;\t\t\t\t\t%% Estimation indicator of the principal point\n',center_optim); +fprintf(fid,'est_alpha = %d;\t\t\t\t\t\t%% Estimation indicator of the skew coefficient\n',est_alpha); +fprintf(fid,'est_dist = [ %d ; %d ; %d ; %d ; %d ];\t%% Estimation indicator of the distortion coefficients\n',est_dist); +fprintf(fid,'\n\n'); +fprintf(fid,'%%-- Extrinsic parameters:\n'); +fprintf(fid,'%%-- The rotation (omc_kk) and the translation (Tc_kk) vectors for every calibration image and their uncertainties\n'); +fprintf(fid,'\n'); +for kk = 1:n_ima, + fprintf(fid,'%%-- Image #%d:\n',kk); + eval(['omckk = omc_' num2str(kk) ';']); + eval(['Tckk = Tc_' num2str(kk) ';']); + fprintf(fid,'omc_%d = [ %d ; %d ; %d ];\n',kk,omckk); + fprintf(fid,'Tc_%d = [ %d ; %d ; %d ];\n',kk,Tckk); + if (exist(['Tc_error_' num2str(kk)])==1) & (exist(['omc_error_' num2str(kk)])==1), + eval(['omckk_error = omc_error_' num2str(kk) ';']); + eval(['Tckk_error = Tc_error_' num2str(kk) ';']); + fprintf(fid,'omc_error_%d = [ %d ; %d ; %d ];\n',kk,omckk_error); + fprintf(fid,'Tc_error_%d = [ %d ; %d ; %d ];\n',kk,Tckk_error); + end; + fprintf(fid,'\n'); +end; + +fclose(fid); \ No newline at end of file diff --git a/src/g_calib/saving_calib_ascii_fisheye.m b/src/g_calib/saving_calib_ascii_fisheye.m new file mode 100755 index 0000000000000000000000000000000000000000..19e7b36de1f454a2d41690a1402270384737c553 --- /dev/null +++ b/src/g_calib/saving_calib_ascii_fisheye.m @@ -0,0 +1,74 @@ +if ~exist('save_name'), + save_name = 'Calib_Results'; +end; + +fprintf(1,'Generating the matlab script file %s.m containing the intrinsic and extrinsic parameters...\n',save_name) + + +fid = fopen([ save_name '.m'],'wt'); + +fprintf(fid,'%% Intrinsic and Extrinsic Camera Parameters\n'); +fprintf(fid,'%%\n'); +fprintf(fid,'%% This script file can be directly excecuted under Matlab to recover the camera intrinsic and extrinsic parameters.\n'); +fprintf(fid,'%% IMPORTANT: This file contains neither the structure of the calibration objects nor the image coordinates of the calibration points.\n'); +fprintf(fid,'%% All those complementary variables are saved in the complete matlab data file Calib_Results.mat.\n'); +fprintf(fid,'%% For more information regarding the calibration model visit http://www.vision.caltech.edu/bouguetj/calib_doc/\n'); +fprintf(fid,'\n\n'); +fprintf(fid,'%%-- Focal length:\n'); +fprintf(fid,'fc = [ %5.15f ; %5.15f ];\n',fc); +fprintf(fid,'\n'); +fprintf(fid,'%%-- Principal point:\n'); +fprintf(fid,'cc = [ %5.15f ; %5.15f ];\n',cc); +fprintf(fid,'\n'); +fprintf(fid,'%%-- Skew coefficient:\n'); +fprintf(fid,'alpha_c = %5.15f;\n',alpha_c); +fprintf(fid,'\n'); +fprintf(fid,'%%-- Distortion coefficients:\n'); +fprintf(fid,'kc = [ %5.15f ; %5.15f ; %5.15f ; %5.15f ];\n',kc); +fprintf(fid,'\n'); +fprintf(fid,'%%-- Focal length uncertainty:\n'); +fprintf(fid,'fc_error = [ %5.15f ; %5.15f ];\n',fc_error); +fprintf(fid,'\n'); +fprintf(fid,'%%-- Principal point uncertainty:\n'); +fprintf(fid,'cc_error = [ %5.15f ; %5.15f ];\n',cc_error); +fprintf(fid,'\n'); +fprintf(fid,'%%-- Skew coefficient uncertainty:\n'); +fprintf(fid,'alpha_c_error = %5.15f;\n',alpha_c_error); +fprintf(fid,'\n'); +fprintf(fid,'%%-- Distortion coefficients uncertainty:\n'); +fprintf(fid,'kc_error = [ %5.15f ; %5.15f ; %5.15f ; %5.15f ];\n',kc_error); +fprintf(fid,'\n'); +fprintf(fid,'%%-- Image size:\n'); +fprintf(fid,'nx = %d;\n',nx); +fprintf(fid,'ny = %d;\n',ny); +fprintf(fid,'\n'); +fprintf(fid,'\n'); +fprintf(fid,'%%-- Various other variables (may be ignored if you do not use the Matlab Calibration Toolbox):\n'); +fprintf(fid,'%%-- Those variables are used to control which intrinsic parameters should be optimized\n'); +fprintf(fid,'\n'); +fprintf(fid,'n_ima = %d;\t\t\t\t\t\t%% Number of calibration images\n',n_ima); +fprintf(fid,'est_fc = [ %d ; %d ];\t\t\t\t\t%% Estimation indicator of the two focal variables\n',est_fc); +fprintf(fid,'est_aspect_ratio = %d;\t\t\t\t%% Estimation indicator of the aspect ratio fc(2)/fc(1)\n',est_aspect_ratio); +fprintf(fid,'center_optim = %d;\t\t\t\t\t%% Estimation indicator of the principal point\n',center_optim); +fprintf(fid,'est_alpha = %d;\t\t\t\t\t\t%% Estimation indicator of the skew coefficient\n',est_alpha); +fprintf(fid,'est_dist = [ %d ; %d ; %d ; %d ; %d ];\t%% Estimation indicator of the distortion coefficients\n',est_dist); +fprintf(fid,'\n\n'); +fprintf(fid,'%%-- Extrinsic parameters:\n'); +fprintf(fid,'%%-- The rotation (omc_kk) and the translation (Tc_kk) vectors for every calibration image and their uncertainties\n'); +fprintf(fid,'\n'); +for kk = 1:n_ima, + fprintf(fid,'%%-- Image #%d:\n',kk); + eval(['omckk = omc_' num2str(kk) ';']); + eval(['Tckk = Tc_' num2str(kk) ';']); + fprintf(fid,'omc_%d = [ %d ; %d ; %d ];\n',kk,omckk); + fprintf(fid,'Tc_%d = [ %d ; %d ; %d ];\n',kk,Tckk); + if (exist(['Tc_error_' num2str(kk)])==1) & (exist(['omc_error_' num2str(kk)])==1), + eval(['omckk_error = omc_error_' num2str(kk) ';']); + eval(['Tckk_error = Tc_error_' num2str(kk) ';']); + fprintf(fid,'omc_error_%d = [ %d ; %d ; %d ];\n',kk,omckk_error); + fprintf(fid,'Tc_error_%d = [ %d ; %d ; %d ];\n',kk,Tckk_error); + end; + fprintf(fid,'\n'); +end; + +fclose(fid); \ No newline at end of file diff --git a/src/g_calib/saving_calib_fisheye.m b/src/g_calib/saving_calib_fisheye.m new file mode 100755 index 0000000000000000000000000000000000000000..7584444a1e71703e60b1a4de2a984fa421236aac --- /dev/null +++ b/src/g_calib/saving_calib_fisheye.m @@ -0,0 +1,220 @@ + +if ~exist('n_ima')|~exist('fc'), + fprintf(1,'No calibration data available.\n'); + return; +end; + +if ~exist('no_image'), + no_image = 0; +end; + +if ~exist('est_alpha'), + est_alpha = 0; +end; + +if ~exist('center_optim'), + center_optim = 1; +end; + +if ~exist('est_aspect_ratio'), + est_aspect_ratio = 1; +end; + +if ~exist('est_fc'); + est_fc = [1;1]; % Set to zero if you do not want to estimate the focal length (it may be useful! believe it or not!) +end; + +%if exist('est_dist'), +% if length(est_dist) == 4, +% est_dist = [est_dist ; 0]; +% end; +%end; + +%if exist('kc'), +% if length(kc) == 4; +% kc = [kc;0]; +% end; +%end; + +if ~exist('fc_error'), + fc_error = NaN*ones(2,1); +end; + +if ~exist('kc_error'), + kc_error = NaN*ones(4,1); +end; + +if ~exist('cc_error'), + cc_error = NaN*ones(2,1); +end; + +if ~exist('alpha_c_error'), + alpha_c_error = NaN; +end; + +check_active_images; + +if ~exist('solution_init'), solution_init = []; end; + +for kk = 1:n_ima, + if ~exist(['dX_' num2str(kk)]), eval(['dX_' num2str(kk) '= dX;']); end; + if ~exist(['dY_' num2str(kk)]), eval(['dY_' num2str(kk) '= dY;']); end; +end; + +if ~exist('solution'), + solution = []; +end; + +if ~exist('param_list'), + param_list = solution; +end; + +if ~exist('wintx'), + wintx = []; + winty = []; +end; + +if ~exist('dX_default'), + dX_default = []; + dY_default = []; +end; + +if ~exist('dX'), + dX = []; +end; +if ~exist('dY'), + dY = dX; +end; + +if ~exist('Hcal'), + Hcal = []; +end; +if ~exist('Wcal'), + Wcal = []; +end; +if ~exist('map'), + map = []; +end; + +if ~exist('wintx_default')|~exist('winty_default'), + wintx_default = max(round(nx/128),round(ny/96)); + winty_default = wintx_default; +end; + +if ~exist('alpha_c'), + alpha_c = 0; +end; + +if ~exist('err_std'), + err_std = [NaN;NaN]; +end; + +for kk = 1:n_ima, + if ~exist(['y_' num2str(kk)]), + eval(['y_' num2str(kk) ' = NaN*ones(2,1);']); + end; + if ~exist(['n_sq_x_' num2str(kk)]), + eval(['n_sq_x_' num2str(kk) ' = NaN;']); + eval(['n_sq_y_' num2str(kk) ' = NaN;']); + end; + if ~exist(['wintx_' num2str(kk)]), + eval(['wintx_' num2str(kk) ' = NaN;']); + eval(['winty_' num2str(kk) ' = NaN;']); + end; + if ~exist(['Tc_' num2str(kk)]), + eval(['Tc_' num2str(kk) ' = [NaN;NaN;NaN];']); + end; + if ~exist(['omc_' num2str(kk)]), + eval(['omc_' num2str(kk) ' = [NaN;NaN;NaN];']); + eval(['Rc_' num2str(kk) ' = NaN * ones(3,3);']); + end; + if ~exist(['omc_error_' num2str(kk)]), + eval(['omc_error_' num2str(kk) ' = NaN*ones(3,1);']); + end; + if ~exist(['Tc_error_' num2str(kk)]), + eval(['Tc_error_' num2str(kk) ' = NaN*ones(3,1);']); + end; + if ~exist(['Rc_' num2str(kk)]), + eval(['Rc_' num2str(kk) ' = rodrigues(omc_' num2str(kk) ');']); + end; +end; + +if ~exist('small_calib_image'), + small_calib_image = 0; +end; + +if ~exist('check_cond'), + check_cond = 1; +end; + +if ~exist('MaxIter'), + MaxIter = 30; +end; + +save_name = 'Calib_Results'; + +if exist([ save_name '.mat'])==2, + disp('WARNING: File Calib_Results.mat already exists'); + if exist('copyfile'), + pfn = -1; + cont = 1; + while cont, + pfn = pfn + 1; + postfix = ['_old' num2str(pfn)]; + save_name = [ 'Calib_Results' postfix]; + cont = (exist([ save_name '.mat'])==2); + end; + copyfile('Calib_Results.mat',[save_name '.mat']); + disp(['Copying the current Calib_Results.mat file to ' save_name '.mat']); + if exist('Calib_Results.m')==2, + copyfile('Calib_Results.m',[save_name '.m']); + disp(['Copying the current Calib_Results.m file to ' save_name '.m']); + end; + cont_save = 1; + else + disp('The file Calib_Results.mat is about to be changed.'); + cont_save = input('Do you want to continue? ([]=no,other=yes) ','s'); + cont_save = ~isempty(cont_save); + end; +else + cont_save = 1; +end; + + +if cont_save, + + save_name = 'Calib_Results'; + + if exist('calib_name'), + + fprintf(1,['\nSaving calibration results under ' save_name '.mat\n']); + + string_save = ['save ' save_name ' center_optim param_list active_images ind_active est_alpha est_dist est_aspect_ratio est_fc fc kc cc alpha_c fc_error kc_error cc_error alpha_c_error err_std ex x y solution solution_init wintx winty n_ima type_numbering N_slots small_calib_image first_num image_numbers format_image calib_name Hcal Wcal nx ny map dX_default dY_default KK inv_KK dX dY wintx_default winty_default no_image check_cond MaxIter']; + + for kk = 1:n_ima, + string_save = [string_save ' X_' num2str(kk) ' x_' num2str(kk) ' y_' num2str(kk) ' ex_' num2str(kk) ' omc_' num2str(kk) ' Rc_' num2str(kk) ' Tc_' num2str(kk) ' omc_error_' num2str(kk) ' Tc_error_' num2str(kk) ' H_' num2str(kk) ' n_sq_x_' num2str(kk) ' n_sq_y_' num2str(kk) ' wintx_' num2str(kk) ' winty_' num2str(kk) ' dX_' num2str(kk) ' dY_' num2str(kk)]; + end; + + else + + fprintf(1,['\nSaving calibration results under ' save_name '.mat (no image version)\n']); + + string_save = ['save ' save_name ' center_optim param_list active_images ind_active est_alpha est_dist est_aspect_ratio est_fc fc kc cc alpha_c fc_error kc_error cc_error alpha_c_error err_std ex x y solution solution_init wintx winty n_ima nx ny dX_default dY_default KK inv_KK dX dY wintx_default winty_default no_image check_cond MaxIter']; + + for kk = 1:n_ima, + string_save = [string_save ' X_' num2str(kk) ' x_' num2str(kk) ' y_' num2str(kk) ' ex_' num2str(kk) ' omc_' num2str(kk) ' Rc_' num2str(kk) ' Tc_' num2str(kk) ' omc_error_' num2str(kk) ' Tc_error_' num2str(kk) ' H_' num2str(kk) ' n_sq_x_' num2str(kk) ' n_sq_y_' num2str(kk) ' wintx_' num2str(kk) ' winty_' num2str(kk) ' dX_' num2str(kk) ' dY_' num2str(kk)]; + end; + + end; + + + + %fprintf(1,'To load later click on Load\n'); + + eval(string_save); + + saving_calib_ascii_fisheye; + + fprintf(1,'done\n'); + +end; diff --git a/src/g_calib/saving_calib_no_results.m b/src/g_calib/saving_calib_no_results.m new file mode 100755 index 0000000000000000000000000000000000000000..49956877081a29e151718ccfc2c77f4609c46b7d --- /dev/null +++ b/src/g_calib/saving_calib_no_results.m @@ -0,0 +1,117 @@ + +if ~exist('n_ima'), + fprintf(1,'No calibration data available.\n'); + return; +end; + +check_active_images; + +for kk = 1:n_ima, + if ~exist(['dX_' num2str(kk)]), eval(['dX_' num2str(kk) '= dX;']); end; + if ~exist(['dY_' num2str(kk)]), eval(['dY_' num2str(kk) '= dY;']); end; +end; + +if ~exist('wintx'), + wintx = []; + winty = []; +end; + +if ~exist('dX_default'), + dX_default = []; + dY_default = []; +end; + +if ~exist('dX'), + dX = []; +end; +if ~exist('dY'), + dY = dX; +end; + + +if ~exist('wintx_default')|~exist('winty_default'), + wintx_default = max(round(nx/128),round(ny/96)); + winty_default = wintx_default; +end; + +if ~exist('alpha_c'), + alpha_c = 0; +end; + +if ~exist('err_std'), + err_std = [NaN;NaN]; +end; + +for kk = 1:n_ima, + + if ~exist(['n_sq_x_' num2str(kk)]), + eval(['n_sq_x_' num2str(kk) ' = NaN;']); + eval(['n_sq_y_' num2str(kk) ' = NaN;']); + end; + if ~exist(['wintx_' num2str(kk)]), + eval(['wintx_' num2str(kk) ' = NaN;']); + eval(['winty_' num2str(kk) ' = NaN;']); + end; +end; + +save_name = 'camera_data'; + +if exist([ save_name '.mat'])==2, + disp('WARNING: File Calib_Results.mat already exists'); + if exist('copyfile')==2, + pfn = -1; + cont = 1; + while cont, + pfn = pfn + 1; + postfix = ['_old' num2str(pfn)]; + save_name = [ 'camera_data' postfix]; + cont = (exist([ save_name '.mat'])==2); + end; + copyfile('camera_data.mat',[save_name '.mat']); + disp(['Copying the current camera_calib_data.mat file to ' save_name '.mat']); + cont_save = 1; + else + disp('The file camera_data.mat is about to be changed.'); + cont_save = input(1,'Do you want to continue? ([]=no,other=yes) ','s'); + cont_save = ~isempty(cont_save); + end; +else + cont_save = 1; +end; + + +if cont_save, + + save_name = 'camera_data'; + + if exist('calib_name'), + + fprintf(1,['\nSaving calibration data under ' save_name '.mat\n']); + + string_save = ['save ' save_name ' active_images ind_active wintx winty n_ima type_numbering N_slots first_num image_numbers format_image calib_name nx ny dX_default dY_default dX dY wintx_default winty_default']; + + for kk = 1:n_ima, + string_save = [string_save ' X_' num2str(kk) ' x_' num2str(kk) ' n_sq_x_' num2str(kk) ' n_sq_y_' num2str(kk) ' wintx_' num2str(kk) ' winty_' num2str(kk) ' dX_' num2str(kk) ' dY_' num2str(kk)]; + end; + + else + + fprintf(1,['\nSaving calibration data under ' save_name '.mat (no image version)\n']); + + string_save = ['save ' save_name ' active_images ind_active wintx winty n_ima nx ny dX_default dY_default dX dY wintx_default winty_default ']; + + for kk = 1:n_ima, + string_save = [string_save ' X_' num2str(kk) ' x_' num2str(kk) ' n_sq_x_' num2str(kk) ' n_sq_y_' num2str(kk) ' wintx_' num2str(kk) ' winty_' num2str(kk) ' dX_' num2str(kk) ' dY_' num2str(kk)]; + end; + + end; + + + + %fprintf(1,'To load later click on Load\n'); + + eval(string_save); + + fprintf(1,'done\n'); + +end; diff --git a/src/g_calib/saving_stereo_calib.m b/src/g_calib/saving_stereo_calib.m new file mode 100755 index 0000000000000000000000000000000000000000..612c80562849defc1d1c6aa2dd7c6f7bbc60d217 --- /dev/null +++ b/src/g_calib/saving_stereo_calib.m @@ -0,0 +1,12 @@ + +fprintf(1,'Saving the stereo calibration results in Calib_Results_stereo.mat...\n'); + +string_save = 'save Calib_Results_stereo om R T recompute_intrinsic_right recompute_intrinsic_left calib_name_left format_image_left type_numbering_left image_numbers_left N_slots_left calib_name_right format_image_right type_numbering_right image_numbers_right N_slots_right fc_left cc_left kc_left alpha_c_left KK_left fc_right cc_right kc_right alpha_c_right KK_right active_images dX dY nx ny n_ima active_images_right active_images_left inconsistent_images center_optim_left est_alpha_left est_dist_left est_fc_left est_aspect_ratio_left center_optim_right est_alpha_right est_dist_right est_fc_right est_aspect_ratio_right history param param_error sigma_x om_error T_error fc_left_error cc_left_error kc_left_error alpha_c_left_error fc_right_error cc_right_error kc_right_error alpha_c_right_error'; + +for kk = 1:n_ima, + if active_images(kk), + string_save = [string_save ' X_left_' num2str(kk) ' omc_left_' num2str(kk) ' Tc_left_' num2str(kk) ' omc_left_error_' num2str(kk) ' Tc_left_error_' num2str(kk) ' n_sq_x_' num2str(kk) ' n_sq_y_' num2str(kk)]; + end; +end; +eval(string_save); + diff --git a/src/g_calib/scanner_calibration_script.m b/src/g_calib/scanner_calibration_script.m new file mode 100755 index 0000000000000000000000000000000000000000..78a905d13e6e838c55046af34c67859bb3993f1b --- /dev/null +++ b/src/g_calib/scanner_calibration_script.m @@ -0,0 +1,466 @@ +%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% +%-- Main 3D Scanner Calibration Script +%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% + +clear; + +%-------------------------------------------------------------------------- +%-- STEP 1: Calibration of the camera independently of the projector: +%-------------------------------------------------------------------------- + +fprintf(1,'STEP 1: Calibration of the camera \n'); + +%% Load the corner coordinates in all the 30 images +load camera_data; + +%% Show one example image: +I_cam1 = imread('cam01.bmp'); + +figure(10); +image(I_cam1); +hold on; +plot(x_1(1,:)+1,x_1(2,:)+1,'r+'); +title('Example camera calibration image: cam01.bmp') +hold off; +drawnow; + +figure(11); +plot3(X_1(1,:),X_1(2,:),X_1(3,:),'r+'); +xlabel('X'); +ylabel('Y'); +zlabel('Z'); +title('3D coordinates of the corresponding points (camera calibration)...') +drawnow; + +% Setup the camera model before calibration: +est_dist = [1;0;0;0;0]; %- A first order distortion model is enough +est_alpha = 0; % No Skew needed +center_optim = 1; % Estimate the principal point + +% Run the main calibration routine: +go_calib_optim; + +% Show the camera location with respect to the patterns: +ext_calib; + +% Saving the camera calibration results under camera_results.mat: +saving_calib; +copyfile('Calib_Results.mat','camera_results.mat'); +delete('Calib_Results.mat'); +delete('Calib_Results.m'); + + +%-- Save the camera calibration results in side variables: +n_ima_cam = n_ima; + +fc_cam = fc; +cc_cam = cc; +kc_cam = kc; +alpha_c_cam = alpha_c; +fc_error_cam = fc_error; +cc_error_cam = cc_error; +kc_error_cam = kc_error; +alpha_c_error_cam = alpha_c_error; + +est_fc_cam = est_fc; +est_dist_cam = est_dist; +est_alpha_cam = est_alpha; +center_optim_cam = center_optim; +nx_cam = nx; +ny_cam = ny; +active_images_cam = active_images; +ind_active_cam = ind_active; + + +X_1_cam = X_1; +x_1_cam = x_1; +omc_1_cam = omc_1; +Rc_1 = rodrigues(omc_1); +Rc_1_cam = Rc_1; +Tc_1_cam = Tc_1; +omc_error_1_cam = omc_error_1; +Tc_error_1_cam = Tc_error_1; + +dX_cam = dX; +dY_cam = dY; + + +clear fc cc kc alpha_c + + +%%% Saving the calibration solution (for further refinement) +param = solution; +param_cam = param([1:10 16:end]); + + + + + +%-------------------------------------------------------------------------- +%-- STEP 2: Calibration of the projector having done the camera calibration: +%-------------------------------------------------------------------------- + +fprintf(1,'STEP 2: Calibration of the projector (having done projector calibration)...\n'); + +% Load the projector data: + +load projector_data; % load the projector corners (previously saved) + +% Show how an example of data: +I_proj1 = imread('proj01n.bmp'); + +figure(20); +image(I_proj1); +hold on; +plot(xproj_1(1,:)+1,xproj_1(2,:)+1,'r+'); +title('Corner locations in image proj01n.bmp') +hold off; +drawnow; + +figure(21); +plot(x_proj_1(1,:)+1,x_proj_1(2,:)+1,'r+'); +title('Corner locations in the projector image plane'); +xlabel('x (in projector image)'); +ylabel('y (in projector image)'); +drawnow; + + + + +% Start projector calibration making use of the information from camera +% calibration: + +X_proj = []; % 3D coordinates of the points +x_proj = []; % 2D coordinates of the points in the projector image +n_ima_proj = []; + +for kk = ind_active, + eval(['xproj = xproj_' num2str(kk) ';']); + xprojn = normalize_pixel(xproj,fc_cam,cc_cam,kc_cam,alpha_c_cam); + eval(['Rc = Rc_' num2str(kk) ';']); + eval(['Tc = Tc_' num2str(kk) ';']); + + Np_proj = size(xproj,2); + Zc = ((Rc(:,3)'*Tc) * (1./(Rc(:,3)' * [xprojn; ones(1,Np_proj)]))); + Xcp = (ones(3,1)*Zc) .* [xprojn; ones(1,Np_proj)]; % % in the camera frame + eval(['X_proj_' num2str(kk) ' = Xcp;']); % coordinates of the points in the + eval(['X_proj = [X_proj X_proj_' num2str(kk) '];']); + eval(['x_proj = [x_proj x_proj_' num2str(kk) '];']); + n_ima_proj = [n_ima_proj kk*ones(1,Np_proj)]; +end; + + +figure(22); +plot(x_proj(1,:)+1,x_proj(2,:)+1,'r+'); +title('Complete set of points in the projector image') +xlabel('x (in projector image)'); +ylabel('y (in projector image)'); +drawnow; + +figure(23); +plot3(X_proj(1,:),X_proj(2,:),X_proj(3,:),'r+'); +axis equal +title('3D coordinates of the projector points (computed using the camera calibration results)') +xlabel('Xc (camera reference frame)'); +ylabel('Yc (camera reference frame)'); +zlabel('Zc (camera reference frame)'); +drawnow; + + +% Projector image size: (may or may not be available) +nx = 1024; +ny = 768; + +% No calibration image is available (only the corner coordinates) +no_image = 1; + +n_ima = 1; +X_1 = X_proj; +x_1 = x_proj; + +% Do estimate distortion: +est_dist = [1 0 0 0 0]'; %ones(5,1); +est_alpha = 0; +center_optim = 1; + + +% Run the main projector calibration routine: +go_calib_optim; + + + +%%% Saving the calibration solution (for further refinement) +param = solution; +param_proj = param([1:10 16:end]); + + + +% Shows the extrinsic parameters: +dX = 30; +dY = 30; +ext_calib; + +% Reprojection on the original images: +dont_ask = 1; +reproject_calib; +dont_ask = 0; + +saving_calib; +copyfile('Calib_Results.mat','projector_results.mat'); +delete('Calib_Results.mat'); +delete('Calib_Results.m'); + + +%-- Projector parameters: + +fc_proj = fc; +cc_proj = cc; +kc_proj = kc; +alpha_c_proj = alpha_c; +fc_error_proj = fc_error; +cc_error_proj = cc_error; +kc_error_proj = kc_error; +alpha_c_error_proj = alpha_c_error; + +est_fc_proj = est_fc; +est_dist_proj = est_dist; +est_alpha_proj = est_alpha; +center_optim_proj = center_optim; +nx_proj = nx; +ny_proj = ny; +active_images_proj = active_images; +ind_active_proj = ind_active; + +% Position of the global structure wrt the projector: +T_proj = Tc_1; +om_proj = omc_1; +R_proj = rodrigues(om_proj); +T_error_proj = Tc_error_1; +om_error_proj = omc_error_1; + + +%-- Restore the camera calibration information (previously saved in local variables) +n_ima = n_ima_cam; +X_1 = X_1_cam; +x_1 = x_1_cam; +no_image = 0; +dX = dX_cam; +dY = dY_cam; + +omc_1 = omc_1_cam; +Rc_1 = Rc_1_cam; +Tc_1 = Tc_1_cam; +omc_error_1 = omc_error_1_cam; +Tc_error_1 = Tc_error_1_cam; + + +%----------------------- Retrieve calibration results: + +% Intrinsic parameters: + +% Projector: +fp = fc_proj; +cp = cc_proj; +kp = kc_proj; +alpha_p = alpha_c_proj; +fp_error = fc_error_proj; +cp_error = cc_error_proj; +kp_error = kc_error_proj; +alpha_p_error = alpha_c_error_proj; + +% Camera: +fc = fc_cam; +cc = cc_cam; +kc = kc_cam; +alpha_c = alpha_c_cam; +fc_error = fc_error_cam; +cc_error = cc_error_cam; +kc_error = kc_error_cam; +alpha_c_error = alpha_c_error_cam; + +% Extrinsic parameters: + +% Relative position of projector and camera: +T = T_proj; +om = om_proj; +R = R_proj; +T_error = T_error_proj; +om_error = om_error_proj; + + +% Relative prosition of camera wrt world (assuming first pattern as reference -- arbitrary): +omc = omc_1_cam; +Rc = Rc_1_cam; +Tc = Tc_1_cam; + +% Relative position of projector wrt world (assuming first pattern as reference -- arbitrary): +Rp = R*Rc; +omp = rodrigues(Rp); +Tp = T + R*Tc; + + +fprintf(1,'Saving the scanner calibration results in calib_cam_proj.mat...\n'); + +saving_string = 'save calib_cam_proj R om T fc fp cc cp alpha_c alpha_p kc kp Rc Rp Tc Tp omc omp n_ima active_images_cam active_images_proj ind_active_cam ind_active_proj T_error om_error fc_error cc_error kc_error alpha_c_error fp_error cp_error kp_error alpha_p_error est_fc_cam est_dist_cam est_alpha_cam center_optim_cam est_fc_proj est_dist_proj est_alpha_proj center_optim_proj param_cam param_proj nx_cam ny_cam nx_proj ny_proj'; + +for kk = 1:n_ima, + saving_string = [saving_string ' X_' num2str(kk) ' x_' num2str(kk) ' xproj_' num2str(kk) ' x_proj_' num2str(kk) ' omc_' num2str(kk) ' Rc_' num2str(kk) ' Tc_' num2str(kk) ' omc_error_' num2str(kk) ' Tc_error_' num2str(kk) ]; +end; + +eval(saving_string); + + + +%-------------------------------------------------------------------------- +%-- STEP 3: Global optimization (optimize over all parameters, camera and projector): +%-------------------------------------------------------------------------- + +fprintf(1,'STEP 3: Global optimization...This step may take a while...\n'); + +string_global = 'global n_ima'; +for kk = 1:n_ima, + string_global = [string_global ' x_' num2str(kk) ' X_' num2str(kk) ' xproj_' num2str(kk) ' x_proj_' num2str(kk)]; +end; +eval(string_global); + + +param = [param_cam([1:4 6 11:end]);param_proj([1:4 6 11:end])]; + + +param_init = param; + + +options = [1 1e-4 1e-4 1e-6 0 0 0 0 0 0 0 0 0 12000 0 1e-8 0.1 0]; +param = leastsq('error_cam_proj3',param,options); + + +%options = optimset('Display','iter','MaxFunEvals',100,'MaxIter',50); +%param = lsqnonlin('error_cam_proj3',param,options); + + + +%-- Retrive the parameters: + +fc = param(1:2); +cc = param(3:4); +alpha_c = 0; +kc = [param(5);zeros(4,1)]; + +for kk = 1:n_ima, + omckk = param(kk*6:kk*6+2); + Tckk = param(kk*6+3:kk*6+5); + Rckk = rodrigues(omckk); + eval(['omc_' num2str(kk) '= omckk;']); + eval(['Tc_' num2str(kk) '= Tckk;']); + eval(['Rc_' num2str(kk) '= Rckk;']); +end; + +fp = param((1:2)+n_ima * 6 + 5); +cp = param((3:4)+n_ima * 6 + 5); +alpha_p = 0; +kp = [param((5)+n_ima * 6 + 5);zeros(4,1)]; + +om = param((6:8)+n_ima * 6 + 5); +T = param((9:11)+n_ima * 6 + 5); +R = rodrigues(om); + +omc = omc_1; +Rc = Rc_1; +Tc = Tc_1; + +Rp = R*Rc; +omp = rodrigues(Rp); +Tp = T + R*Tc; + + +%-- Re-create the parameters: + +param_cam = [param(1:4);0;param(5);zeros(4,1);param(6:5+6*n_ima)]; +param_proj = [param((1:4)+5+6*n_ima);0;param(5+5+6*n_ima);zeros(4,1);param((6:11)+5+6*n_ima)]; + + +fprintf(1,'Saving the optimized scanner calibration results in calib_cam_proj_optim.mat...\n'); + +saving_string = 'save calib_cam_proj_optim R om T fc fp cc cp alpha_c alpha_p kc kp Rc Rp Tc Tp omc omp n_ima active_images_cam active_images_proj ind_active_cam ind_active_proj est_fc_cam est_dist_cam est_alpha_cam center_optim_cam est_fc_proj est_dist_proj est_alpha_proj center_optim_proj param_cam param_proj param_init nx_cam ny_cam nx_proj ny_proj'; + +for kk = 1:n_ima, + saving_string = [saving_string ' X_' num2str(kk) ' x_' num2str(kk) ' xproj_' num2str(kk) ' x_proj_' num2str(kk) ' omc_' num2str(kk) ' Rc_' num2str(kk) ' Tc_' num2str(kk) ]; +end; + +eval(saving_string); + + +% Save the optimal camera parameters: + +no_image = 0; + +nx = nx_cam; +ny = ny_cam; + +comp_error_calib; + +saving_calib; +copyfile('Calib_Results.mat','camera_results_optim.mat'); +delete('Calib_Results.mat'); +delete('Calib_Results.m'); + + +omc_1_cam = omc_1; +Rc_1_cam = Rc_1; +Tc_1_cam = Tc_1; + +% Save the optimal projector parameters: + + +X_proj = []; +x_proj = []; + +for kk = 1:n_ima, + eval(['xproj = xproj_' num2str(kk) ';']); + xprojn = normalize_pixel(xproj,fc,cc,kc,alpha_c); + eval(['Rc = Rc_' num2str(kk) ';']); + eval(['Tc = Tc_' num2str(kk) ';']); + Np_proj = size(xproj,2); + Zc = ((Rc(:,3)'*Tc) * (1./(Rc(:,3)' * [xprojn; ones(1,Np_proj)]))); + Xcp = (ones(3,1)*Zc) .* [xprojn; ones(1,Np_proj)]; % % in the camera frame + eval(['X_proj_' num2str(kk) ' = Xcp;']); % coordinates of the points in the + eval(['X_proj = [X_proj X_proj_' num2str(kk) '];']); + eval(['x_proj = [x_proj x_proj_' num2str(kk) '];']); +end; + +fc = fp; +cc = cp; +alpha_c = alpha_p; +kc = kp; + +n_ima = 1; +X_1 = X_proj; +x_1 = x_proj; +omc_1 = om; +Tc_1 = T; +Rc_1 = R; + +% Image size: (may or may not be available) +nx = nx_proj; +ny = ny_proj; + +% No calibration image is available (only the corner coordinates) +no_image = 1; + +comp_error_calib; + +saving_calib; +copyfile('Calib_Results.mat','projector_results_optim.mat'); +delete('Calib_Results.mat'); +delete('Calib_Results.m'); + +n_ima = n_ima_cam; +X_1 = X_1_cam; +x_1 = x_1_cam; +no_image = 0; +dX = dX_cam; +dY = dY_cam; + +omc_1 = omc_1_cam; +Rc_1 = Rc_1_cam; +Tc_1 = Tc_1_cam; \ No newline at end of file diff --git a/src/g_calib/scanning_script.m b/src/g_calib/scanning_script.m new file mode 100755 index 0000000000000000000000000000000000000000..a3cee7e9d633aac42c1b1e41f7a8ec4dfc60f653 --- /dev/null +++ b/src/g_calib/scanning_script.m @@ -0,0 +1,75 @@ +%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% +%-- Main 3D Scanning Script +%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% + + +if exist('calib_cam_proj_optim.mat')~=2, + error('The scanner calibration file does not exist. Make sure to go to the directory where the scanning images and the calibration file are located (folder scanning_example)'); +end; + +% Loading the scanner calibration parameters: +fprintf(1,'Loading the scanner calibration data...\n'); +load calib_cam_proj_optim; + + +% Choose a dataset (scan #20 for example) +ind_view = 20; +stripe_image = ['strip' sprintf('%.4d',ind_view) ]; + +if exist([stripe_image '_pat00p.bmp'])~=2, + error('The scanning images cannot be found. Make sure to go to the directory where the scanning images are located (folder scanning_example)'); +end; + + +% Compute the projector coordinates at every pixel in the camera image: +fprintf(1,'Computing the subpixel projector coordinates at every pixel in the camera image...\n'); +[xc,xp,nx,ny] = ComputeStripes(stripe_image,1); + + +% Triangulate the 3D geometry: +fprintf(1,'Triangulating the 3D geometry...\n'); +[Xc,Xp] = Compute3D(xc,xp,R,T,fc,fp,cc,cp,kc,kp,alpha_c,alpha_p); + + +% Meshing the points: +Thresh_connect = 15; +N_smoothing = 1; +fprintf(1,'Computing the 3D surface mesh...\n'); +[Xc2,tri2,xc2,xp2,dc2,xc_texture,nc2,conf_nc2,Nn2] = Meshing(Xc,xc,xp,Thresh_connect,N_smoothing,om,T,nx,ny,fc,cc,kc,alpha_c,fp,cp,kp,alpha_p); + + +% Display the 3D mesh: +figure(7); +h = trimesh(tri2,Xc2(1,:),Xc2(3,:),-Xc2(2,:)); +set(h,'EdgeColor', 'b'); +xlabel('X'); +ylabel('Y'); +zlabel('Z'); +axis('equal'); +rotate3d on; +view(0.5,12); +axis equal + + + +% Save the mesh in a VRML file: +TT = [0;0;0]; +wwn = [1;0;0]; +nw = pi; +fieldOfView = 3*atan((ny/2)/fc(2)); + +filename_VRML = ['mesh' sprintf('%.4d',ind_view) '.wrl']; + +fprintf(1,'Saving Geometry in the VRML file %s...(use Cosmo Player to visualize the mesh)\n',filename_VRML); + +file = fopen(filename_VRML,'wt'); +fprintf(file ,'#VRML V2.0 utf8\n'); +fprintf(file,'Transform { children [ Viewpoint {position %.4f %.4f %.4f orientation %.4f %.4f %.4f %.4f fieldOfView %.4f } \n',[TT ; wwn ; nw; fieldOfView]); +%fprintf(file,'Transform { children [ Viewpoint {position %.4f %.4f %.4f orientation %.4f %.4f %.4f %.4f fieldOfView %.4f } \n',[TT ; wwn ; 0; atan((ny/2)/fc(2))]); +fprintf(file ,'Transform { children [ Shape { appearance Appearance { material Material { }} geometry IndexedFaceSet { coord Coordinate { point [\n'); +fprintf(file,'%.3f %.3f %.3f,\n',Xc2); +fprintf(file ,']} coordIndex [\n'); +fprintf(file,'%d,%d,%d,-1,\n',tri2'-1); +fprintf(file ,']}}]}\n'); +fprintf(file,']} '); +fclose(file) ; diff --git a/src/g_calib/script_fit_distortion.m b/src/g_calib/script_fit_distortion.m new file mode 100755 index 0000000000000000000000000000000000000000..c5e54302b01c4137e3404e377c3bac02b9e9834c --- /dev/null +++ b/src/g_calib/script_fit_distortion.m @@ -0,0 +1,39 @@ + + satis_distort = 0; + + disp(['Estimated focal: ' num2str(f_g) ' pixels']); + + while ~satis_distort, + + k_g = input('Guess for distortion factor kc ([]=0): '); + + if isempty(k_g), k_g = 0; end; + + xy_corners_undist = comp_distortion2([x' - c_g(1);y'-c_g(2)]/f_g,k_g); + + xu = xy_corners_undist(1,:)'; + yu = xy_corners_undist(2,:)'; + + [XXu] = projectedGrid ( [xu(1);yu(1)], [xu(2);yu(2)],[xu(3);yu(3)], [xu(4);yu(4)],n_sq_x+1,n_sq_y+1); % The full grid + + XX = (ones(2,1)*(1 + k_g * sum(XXu.^2))) .* XXu; + XX(1,:) = f_g*XX(1,:)+c_g(1); + XX(2,:) = f_g*XX(2,:)+c_g(2); + + figure(2); + image(I); + colormap(map); + zoom on; + hold on; + %plot(f_g*XXu(1,:)+c_g(1),f_g*XXu(2,:)+c_g(2),'ro'); + plot(XX(1,:),XX(2,:),'r+'); + title('The red crosses should be on the grid corners...'); + hold off; + + satis_distort = input('Satisfied with distortion? ([]=no, other=yes) '); + + satis_distort = ~isempty(satis_distort); + + + end; + \ No newline at end of file diff --git a/src/g_calib/script_fit_distortion_fisheye.m b/src/g_calib/script_fit_distortion_fisheye.m new file mode 100755 index 0000000000000000000000000000000000000000..ef216deebf5722cad9928d1a4793b2c99cc439f2 --- /dev/null +++ b/src/g_calib/script_fit_distortion_fisheye.m @@ -0,0 +1,65 @@ + + satis_distort = 0; + + while ~satis_distort, + + k_g_save = k_g; + + k_g = input(['Guess for distortion factor kc ([]=' num2str(k_g_save) '): ']); + + if isempty(k_g), k_g = k_g_save; end; + + +x_n = (x - c_g(1))/f_g; +y_n = (y - c_g(2))/f_g; + +[x_pn] = comp_fisheye_distortion([x_n' ; y_n'],[k_g;0;0;0]); + + +% Compute the inside points through computation of the planar homography (collineation) + +a00 = [x_pn(1,1);x_pn(2,1);1]; +a10 = [x_pn(1,2);x_pn(2,2);1]; +a11 = [x_pn(1,3);x_pn(2,3);1]; +a01 = [x_pn(1,4);x_pn(2,4);1]; + +% Compute the planar collineation: (return the normalization matrix as well) +[Homo,Hnorm,inv_Hnorm] = compute_homography([a00 a10 a11 a01],[0 1 1 0;0 0 1 1;1 1 1 1]); + + +% Build the grid using the planar collineation: + +x_l = ((0:n_sq_x)'*ones(1,n_sq_y+1))/n_sq_x; +y_l = (ones(n_sq_x+1,1)*(0:n_sq_y))/n_sq_y; +pts = [x_l(:) y_l(:) ones((n_sq_x+1)*(n_sq_y+1),1)]'; + +XXpn = Homo*pts; +XXpn = XXpn(1:2,:) ./ (ones(2,1)*XXpn(3,:)); + +XX = apply_fisheye_distortion(XXpn,[k_g;0;0;0]); + +XX(1,:) = f_g*XX(1,:) + c_g(1); +XX(2,:) = f_g*XX(2,:) + c_g(2); + + + + + + + + figure(2); + image(I); + colormap(map); + zoom on; + hold on; + plot(XX(1,:),XX(2,:),'r+'); + title('The red crosses should be on the grid corners...'); + hold off; + + satis_distort = input('Satisfied with distortion? ([]=no, other=yes) '); + + satis_distort = ~isempty(satis_distort); + + + end; + \ No newline at end of file diff --git a/src/g_calib/show_calib_results.m b/src/g_calib/show_calib_results.m new file mode 100755 index 0000000000000000000000000000000000000000..e2e306becf72d2eb5144b24bc691060864d12c9f --- /dev/null +++ b/src/g_calib/show_calib_results.m @@ -0,0 +1,57 @@ +% Color code for each image: + +if ~exist('n_ima')|~exist('fc'), + fprintf(1,'No calibration data available.\n'); + return; +end; + +check_active_images; + +if ~exist('alpha_c'), + alpha_c = 0; + est_alpha = 0; +end; + +if length(kc) == 4; + kc = [kc;0]; +end; + +if ~exist('est_dist'), + est_dist = (kc~=0); +else + if length(est_dist) == 4; + est_dist = [est_dist;0]; + end; +end; + +if ~exist('err_std'), + comp_error_calib; +end; + + +if ~exist('fc_error'), + + fprintf(1,'\n\nCalibration results:\n\n'); + fprintf(1,'Focal Length: fc = [ %3.5f %3.5f ]\n',[fc]); + fprintf(1,'Principal point: cc = [ %3.5f %3.5f ]\n',[cc]); + fprintf(1,'Skew: alpha_c = [ %3.5f ] => angle of pixel axes = %3.5f degrees\n',[alpha_c],90 - atan(alpha_c)*180/pi); + fprintf(1,'Distortion: kc = [ %3.5f %3.5f %3.5f %3.5f %5.5f ]\n',[kc]); + if n_ima ~= 0, + fprintf(1,'Pixel error: err = [ %3.5f %3.5f ]\n',err_std); + end; + fprintf(1,'\n\n\n'); + +else + + fprintf(1,'\n\nCalibration results (with uncertainties):\n\n'); + fprintf(1,'Focal Length: fc = [ %3.5f %3.5f ] ± [ %3.5f %3.5f ]\n',[fc;fc_error]); + fprintf(1,'Principal point: cc = [ %3.5f %3.5f ] ± [ %3.5f %3.5f ]\n',[cc;cc_error]); + fprintf(1,'Skew: alpha_c = [ %3.5f ] ± [ %3.5f ] => angle of pixel axes = %3.5f ± %3.5f degrees\n',[alpha_c;alpha_c_error],90 - atan(alpha_c)*180/pi,atan(alpha_c_error)*180/pi); + fprintf(1,'Distortion: kc = [ %3.5f %3.5f %3.5f %3.5f %5.5f ] ± [ %3.5f %3.5f %3.5f %3.5f %5.5f ]\n',[kc;kc_error]); + if n_ima ~= 0, + fprintf(1,'Pixel error: err = [ %3.5f %3.5f ]\n',err_std); + end; + fprintf(1,'\n',err_std); + fprintf(1,'Note: The numerical errors are approximately three times the standard deviations (for reference).\n\n\n') + +end; diff --git a/src/g_calib/show_calib_results_fisheye.m b/src/g_calib/show_calib_results_fisheye.m new file mode 100755 index 0000000000000000000000000000000000000000..bae5cc493d3228a3b83ca3ba0c2f28ad8ef14a81 --- /dev/null +++ b/src/g_calib/show_calib_results_fisheye.m @@ -0,0 +1,57 @@ +% Color code for each image: + +if ~exist('n_ima')|~exist('fc'), + fprintf(1,'No calibration data available.\n'); + return; +end; + +check_active_images; + +if ~exist('alpha_c'), + alpha_c = 0; + est_alpha = 0; +end; + +%if length(kc) == 4; +% kc = [kc;0]; +%end; + +%if ~exist('est_dist'), +% est_dist = (kc~=0); +%else +% if length(est_dist) == 4; +% est_dist = [est_dist;0]; +% end; +%end; + +if ~exist('err_std'), + comp_error_calib_fisheye; +end; + + +if ~exist('fc_error'), + + fprintf(1,'\n\nCalibration results:\n\n'); + fprintf(1,'Focal Length: fc = [ %3.5f %3.5f ]\n',[fc]); + fprintf(1,'Principal point: cc = [ %3.5f %3.5f ]\n',[cc]); + fprintf(1,'Skew: alpha_c = [ %3.5f ] => angle of pixel axes = %3.5f degrees\n',[alpha_c],90 - atan(alpha_c)*180/pi); + fprintf(1,'Fisheye Distortion: kc = [ %3.5f %3.5f %3.5f %3.5f ]\n',[kc]); + if n_ima ~= 0, + fprintf(1,'Pixel error: err = [ %3.5f %3.5f ]\n',err_std); + end; + fprintf(1,'\n\n\n'); + +else + + fprintf(1,'\n\nCalibration results (with uncertainties):\n\n'); + fprintf(1,'Focal Length: fc = [ %3.5f %3.5f ] ± [ %3.5f %3.5f ]\n',[fc;fc_error]); + fprintf(1,'Principal point: cc = [ %3.5f %3.5f ] ± [ %3.5f %3.5f ]\n',[cc;cc_error]); + fprintf(1,'Skew: alpha_c = [ %3.5f ] ± [ %3.5f ] => angle of pixel axes = %3.5f ± %3.5f degrees\n',[alpha_c;alpha_c_error],90 - atan(alpha_c)*180/pi,atan(alpha_c_error)*180/pi); + fprintf(1,'Fisheye Distortion: kc = [ %3.5f %3.5f %3.5f %3.5f ] ± [ %3.5f %3.5f %3.5f %3.5f ]\n',[kc;kc_error]); + if n_ima ~= 0, + fprintf(1,'Pixel error: err = [ %3.5f %3.5f ]\n',err_std); + end; + fprintf(1,'\n',err_std); + fprintf(1,'Note: The numerical errors are approximately three times the standard deviations (for reference).\n\n\n') + +end; diff --git a/src/g_calib/show_stereo_calib_results.m b/src/g_calib/show_stereo_calib_results.m new file mode 100755 index 0000000000000000000000000000000000000000..16cfb24ded22f17d29e538763ad3cd830fc39b0f --- /dev/null +++ b/src/g_calib/show_stereo_calib_results.m @@ -0,0 +1,28 @@ + +fprintf(1,'\nStereo calibration parameters:\n'); + + +fprintf(1,'\n\nIntrinsic parameters of left camera:\n\n'); +fprintf(1,'Focal Length: fc_left = [ %3.5f %3.5f ] ± [ %3.5f %3.5f ]\n',[fc_left;fc_left_error]); +fprintf(1,'Principal point: cc_left = [ %3.5f %3.5f ] ± [ %3.5f %3.5f ]\n',[cc_left;cc_left_error]); +fprintf(1,'Skew: alpha_c_left = [ %3.5f ] ± [ %3.5f ] => angle of pixel axes = %3.5f ± %3.5f degrees\n',[alpha_c_left;alpha_c_left_error],90 - atan(alpha_c_left)*180/pi,atan(alpha_c_left_error)*180/pi); +fprintf(1,'Distortion: kc_left = [ %3.5f %3.5f %3.5f %3.5f %5.5f ] ± [ %3.5f %3.5f %3.5f %3.5f %5.5f ]\n',[kc_left;kc_left_error]); + + +fprintf(1,'\n\nIntrinsic parameters of right camera:\n\n'); +fprintf(1,'Focal Length: fc_right = [ %3.5f %3.5f ] ± [ %3.5f %3.5f ]\n',[fc_right;fc_right_error]); +fprintf(1,'Principal point: cc_right = [ %3.5f %3.5f ] ± [ %3.5f %3.5f ]\n',[cc_right;cc_right_error]); +fprintf(1,'Skew: alpha_c_right = [ %3.5f ] ± [ %3.5f ] => angle of pixel axes = %3.5f ± %3.5f degrees\n',[alpha_c_right;alpha_c_right_error],90 - atan(alpha_c_right)*180/pi,atan(alpha_c_right_error)*180/pi); +fprintf(1,'Distortion: kc_right = [ %3.5f %3.5f %3.5f %3.5f %5.5f ] ± [ %3.5f %3.5f %3.5f %3.5f %5.5f ]\n',[kc_right;kc_right_error]); + + +fprintf(1,'\n\nExtrinsic parameters (position of right camera wrt left camera):\n\n'); +if ~exist('om_error')|~exist('T_error'), + fprintf(1,'Rotation vector: om = [ %3.5f %3.5f %3.5f ]\n',[om]); + fprintf(1,'Translation vector: T = [ %3.5f %3.5f %3.5f ]\n',[T]); +else + fprintf(1,'Rotation vector: om = [ %3.5f %3.5f %3.5f ] ± [ %3.5f %3.5f %3.5f ]\n',[om;om_error]); + fprintf(1,'Translation vector: T = [ %3.5f %3.5f %3.5f ] ± [ %3.5f %3.5f %3.5f ]\n',[T;T_error]); +end; + +fprintf(1,'\n\nNote: The numerical errors are approximately three times the standard deviations (for reference).\n\n') diff --git a/src/g_calib/show_window.m b/src/g_calib/show_window.m new file mode 100755 index 0000000000000000000000000000000000000000..55e053c91dabb4ff072e8f67adc879679fad01cb --- /dev/null +++ b/src/g_calib/show_window.m @@ -0,0 +1,77 @@ +function show_window(cell_list,fig_number,title_figure,x_size,y_size,gap_x,font_name,font_size) + + +if ~exist('cell_list'), + error('No description of the functions'); +end; + +if ~exist('fig_number'), + fig_number = 1; +end; +if ~exist('title_figure'), + title_figure = ''; +end; +if ~exist('x_size'), + x_size = 85; +end; +if ~exist('y_size'), + y_size = 14; +end; +if ~exist('gap_x'), + gap_x = 0; +end; +if ~exist('font_name'), + font_name = 'clean'; +end; +if ~exist('font_size'), + font_size = 8; +end; + +figure(fig_number); clf; +pos = get(fig_number,'Position'); + +[n_row,n_col] = size(cell_list); + +fig_size_x = x_size*n_col+(n_col+1)*gap_x; +fig_size_y = y_size*n_row+(n_row+1)*gap_x; + +set(fig_number,'Units','points', ... + 'BackingStore','off', ... + 'Color',[0.8 0.8 0.8], ... + 'MenuBar','none', ... + 'Resize','off', ... + 'Name',title_figure, ... +'Position',[pos(1) pos(2) fig_size_x fig_size_y], ... +'NumberTitle','off'); %,'WindowButtonMotionFcn',['figure(' num2str(fig_number) ');']); + +h_mat = zeros(n_row,n_col); + +posx = zeros(n_row,n_col); +posy = zeros(n_row,n_col); + +for i=n_row:-1:1, + for j = n_col:-1:1, + posx(i,j) = gap_x+(j-1)*(x_size+gap_x); + posy(i,j) = fig_size_y - i*(gap_x+y_size); + end; +end; + +for i=n_row:-1:1, + for j = n_col:-1:1, + if ~isempty(cell_list{i,j}), + if ~isempty(cell_list{i,j}{1}) & ~isempty(cell_list{i,j}{2}), + h_mat(i,j) = uicontrol('Parent',fig_number, ... + 'Units','points', ... + 'Callback',cell_list{i,j}{2}, ... + 'ListboxTop',0, ... + 'Position',[posx(i,j) posy(i,j) x_size y_size], ... + 'String',cell_list{i,j}{1}, ... + 'fontsize',font_size,... + 'fontname',font_name,... + 'Tag','Pushbutton1'); + end; + end; + end; +end; + +%------ END PROTECTED REGION ----------------% diff --git a/src/g_calib/skew3.m b/src/g_calib/skew3.m new file mode 100755 index 0000000000000000000000000000000000000000..449a13b9ed92af6fc08f56e0cc5255d88c3ee3a8 --- /dev/null +++ b/src/g_calib/skew3.m @@ -0,0 +1,27 @@ +function [S,dSdT] = skew3(T); + +S = [ 0 -T(3) T(2) + T(3) 0 -T(1) + -T(2) T(1) 0 ]; + +dSdT = [0 0 0;0 0 1;0 -1 0 ;0 0 -1;0 0 0;1 0 0 ;0 1 0;-1 0 0; 0 0 0]; + +return; + + +% Test of Jacobian: + +T1 = randn(3,1); + +dT = 0.001*randn(3,1); + +T2 = T1 + dT; + +[S1,dSdT] = skew3(T1); +[S2] = skew3(T2); + +S2app = S1; +S2app(:) = S2app(:) + dSdT*dT; + + +norm(S1 - S2) / norm(S2app - S2) diff --git a/src/g_calib/small_test_script.m b/src/g_calib/small_test_script.m new file mode 100755 index 0000000000000000000000000000000000000000..97ddf31d3d5da172377b79106d85dfc3c8164d21 --- /dev/null +++ b/src/g_calib/small_test_script.m @@ -0,0 +1,65 @@ +% This is small script that demonstrate the computation of extrinsic +% parameters using 3D structures. +% This test was build from data provided by Daniel Small (thank you Daniel!) + + +%-- Image points (in pixels): + +x = [479.5200 236.0800 + 608.4100 415.3700 + 461.0000 40.0000 + 451.4800 308.7000 + 373.9900 314.8900 + 299.3200 319.1300 + 231.5500 321.3700 + 443.7300 282.9200 + 378.3600 288.3000 + 314.6900 292.7400 + 255.4700 296.2300]'; + + +% 3D world coordinates: + +X = [ 0 0 0 + 54.0000 0 0 + 0 0 40.5000 + 27.0000 -8.4685 -2.3750 + 27.0000 -18.4685 -2.3750 + 27.0000 -28.4685 -2.3750 + 27.0000 -38.4685 -2.3750 + 17.0000 -8.4685 -2.3750 + 17.0000 -18.4685 -2.3750 + 17.0000 -28.4685 -2.3750 + 17.0000 -38.4685 -2.3750]'; + + +%------------ Intrinsic parameters: +%--- focal: +fc = [ 395.0669 357.1178 ]'; +%--- Principal point: +cc = [ 380.5387 230.5278 ]'; +%--- Distortion coefficients: +kc = [-0.2601 0.0702 -0.0019 -0.0003 0]'; +%--- Skew coefficient: +alpha_c = 0; + +%----- Computation of the pose of the object in space +%----- (or the rigid motion between world reference frame and camera ref. frame) +[om,T,R] = compute_extrinsic(x,X,fc,cc,kc,alpha_c); + +%--- Try to reproject the structure to see if the computed pose makes sense: +x2 = project_points2(X_1,omckk,Tckk,fc,cc,kc,alpha_c); + + +% Graphical output: +figure(2); +plot(x(1,:),x(2,:),'r+'); +hold on; +plot(x2(1,:),x2(2,:),'go'); +hold off; +axis('equal'); +axis('image'); +title('red crosses: data, green circles: reprojected structure -- IT WORKS!!!'); +xlabel('x'); +ylabel('y'); + diff --git a/src/g_calib/smooth_images.m b/src/g_calib/smooth_images.m new file mode 100755 index 0000000000000000000000000000000000000000..e570b5693b0819753006f76b2fd3b743d31bae0d --- /dev/null +++ b/src/g_calib/smooth_images.m @@ -0,0 +1,94 @@ +% Anisotropically diffuse the calibration image +% to enhance the corner detection + + +fprintf(1,'Anisotropic diffusion of the images for corner enhancement (the images have to be loaded in memory)\n'); + + +ker = [1/4 1/2 1/4]; +ker2 = conv2(ker,ker); +ker2 = conv2(ker2,ker); +ker2 = conv2(ker2,ker); + + + +if ~exist(['I_' num2str(ind_active(1))]), + ima_read_calib; +end; + +check_active_images; + +format_image2 = format_image; +if format_image2(1) == 'j', + format_image2 = 'bmp'; +end; + +for kk = 1:n_ima, + + if exist(['I_' num2str(kk)]), + + %fprintf(1,'%d...',kk); + + eval(['I = I_' num2str(kk) ';']); + + + % Compute the sigI automatically: + [nn,xx] = hist(I(:),50); + nn = conv2(nn,ker2,'same'); + + max_nn = max(nn); + + + localmax = [0 (nn(2:end-1)>=nn(3:end)) & (nn(2:end-1) > nn(1:end-2)) 0] .* (nn >= max_nn/5); + + %plot(xx,nn); + %hold on; + %plot(xx,nn .* localmax,'r' ); + %hold off; + + localmax_ind = find(localmax); + nn_local_max = nn(localmax_ind); + + % order the picks: + [a,b] = sort(-nn_local_max); + + localmax_ind = localmax_ind(b); + nn_local_max = nn_local_max(b); + + sig_I = abs(xx(localmax_ind(1)) - xx(localmax_ind(2)))/4.25; + + + + + I2 = anisdiff(I,sig_I,30); + + + if ~type_numbering, + number_ext = num2str(image_numbers(kk)); + else + number_ext = sprintf(['%.' num2str(N_slots) 'd'],image_numbers(kk)); + end; + + ima_name2 = [calib_name '_smth' number_ext '.' format_image2]; + + fprintf(1,['Saving smoothed image under ' ima_name2 '...\n']); + + if format_image2(1) == 'p', + if format_images2(2) == 'p', + saveppm(ima_name2,uint8(round(I2))); + else + savepgm(ima_name2,uint8(round(I2))); + end; + else + if format_image2(1) == 'r', + writeras(ima_name2,round(I2),gray(256)); + else + imwrite(uint8(round(I2)),gray(256),ima_name2,format_image2); + end; + end; + + end; + +end; + +fprintf(1,'\ndone\n'); \ No newline at end of file diff --git a/src/g_calib/startup.m b/src/g_calib/startup.m new file mode 100755 index 0000000000000000000000000000000000000000..e12abf9c77659648f2537277815f1a199b77a676 --- /dev/null +++ b/src/g_calib/startup.m @@ -0,0 +1,2 @@ +% Main camera calibration toolbox: +format compact diff --git a/src/g_calib/stereo_gui.m b/src/g_calib/stereo_gui.m new file mode 100755 index 0000000000000000000000000000000000000000..0197d3e4b9173da61d2be95732629a246b0cbcc2 --- /dev/null +++ b/src/g_calib/stereo_gui.m @@ -0,0 +1,155 @@ +% stereo_gui +% Stereo Camera Calibration Toolbox (two cameras, internal and external calibration): +% +% It is assumed that the two cameras (left and right) have been calibrated with the pattern at the same 3D locations, and the same points +% on the pattern (select the same grid points). Therefore, in particular, the same number of images were used to calibrate both cameras. +% The two calibration result files must have been saved under two seperate data files (Calib_Results_left.mat and Calib_Results_right.mat) +% prior to running this toolbox. To generate the two files, run the classic Camera Calibration toolbox calib.m. +% +% INPUT: Calib_Results_left.mat, Calib_Results_right.mat -> Generated by the standard calibration toolbox on the two cameras individually +% OUTPUT: Calib_Results_stereo.mat -> The saved result after global stereo calibration (after running stereo calibration, and hitting Save stereo calib results) +% +% Main result variables stored in Calib_Results_stereo.mat: +% om, R, T: relative rotation and translation of the right camera wrt the left camera +% fc_left, cc_left, kc_left, alpha_c_left, KK_left: New intrinsic parameters of the left camera +% fc_right, cc_right, kc_right, alpha_c_right, KK_right: New intrinsic parameters of the right camera +% +% Both sets of intrinsic parameters are equivalent to the classical {fc,cc,kc,alpha_c,KK} described online at: +% http://www.vision.caltech.edu/bouguetj/calib_doc/parameters.html +% +% Note: If you do not want to recompute the intinsic parameters, through stereo calibration you may want to set +% recompute_intrinsic_right and recompute_intrinsic_left to zero, prior to running stereo calibration. Default: 1 +% +% Definition of the extrinsic parameters: R and om are related through the rodrigues formula (R=rodrigues(om)). +% Consider a point P of coordinates XL and XR in the left and right camera reference frames respectively. +% XL and XR are related to each other through the following rigid motion transformation: +% XR = R * XL + T +% R and T (or equivalently om and T) fully describe the relative displacement of the two cameras. +% +% +% If the Warning message "Disabling view kk - Reason: the left and right images are found inconsistent" is encountered during stereo calibration, +% that probably means that for the kkth pair of images, the left and right images are found to have captured the calibration pattern at two +% different locations in space. That means that the two views are not consistent, and therefore cannot be used for stereo calibration. +% When capturing your images, make sure that you do not move the calibration pattern between capturing the left and the right images. +% The pattwern can (and should) be moved in space only between two sets of (left,right) images. +% Another reason for inconsistency is that you selected a different set of points on the pattern when running the separate calibrations +% (leading to the two files Calib_Results_left.mat and Calib_Results_left.mat). Make sure that the same points are selected in the +% two separate calibration. In other words, the points need to correspond. + +% (c) Jean-Yves Bouguet - Intel Corporation +% October 25, 2001 -- Last updated June 14, 2004 + +function stereo_gui, + + +cell_list = {}; + + +%-------- Begin editable region -------------% +%-------- Begin editable region -------------% + + +fig_number = 1; + +title_figure = 'Stereo Camera Calibration Toolbox'; + +cell_list{1,1} = {'Load left and right calibration files','load_stereo_calib_files;'}; +cell_list{1,2} = {'Run stereo calibration','go_calib_stereo;'}; +cell_list{2,1} = {'Show Extrinsics of stereo rig','ext_calib_stereo;'}; +cell_list{2,2} = {'Show Intrinsic parameters','show_stereo_calib_results;'}; +cell_list{3,1} = {'Save stereo calib results','saving_stereo_calib;'}; +cell_list{3,2} = {'Load stereo calib results','loading_stereo_calib;'}; +cell_list{4,1} = {'Rectify the calibration images','rectify_stereo_pair;'}; +cell_list{4,2} = {'Exit',['disp(''Bye. To run again, type stereo_gui.''); close(' num2str(fig_number) ');']}; %{'Exit','calib_gui;'}; + + +show_window(cell_list,fig_number,title_figure,150,14); + + +%-------- End editable region -------------% +%-------- End editable region -------------% + + + + + + +%------- DO NOT EDIT ANYTHING BELOW THIS LINE -----------% + +function show_window(cell_list,fig_number,title_figure,x_size,y_size,gap_x,font_name,font_size) + + +if ~exist('cell_list'), + error('No description of the functions'); +end; + +if ~exist('fig_number'), + fig_number = 1; +end; +if ~exist('title_figure'), + title_figure = ''; +end; +if ~exist('x_size'), + x_size = 85; +end; +if ~exist('y_size'), + y_size = 14; +end; +if ~exist('gap_x'), + gap_x = 0; +end; +if ~exist('font_name'), + font_name = 'clean'; +end; +if ~exist('font_size'), + font_size = 8; +end; + +figure(fig_number); clf; +pos = get(fig_number,'Position'); + +[n_row,n_col] = size(cell_list); + +fig_size_x = x_size*n_col+(n_col+1)*gap_x; +fig_size_y = y_size*n_row+(n_row+1)*gap_x; + +set(fig_number,'Units','points', ... + 'BackingStore','off', ... + 'Color',[0.8 0.8 0.8], ... + 'MenuBar','none', ... + 'Resize','off', ... + 'Name',title_figure, ... +'Position',[pos(1) pos(2) fig_size_x fig_size_y], ... +'NumberTitle','off'); %,'WindowButtonMotionFcn',['figure(' num2str(fig_number) ');']); + +h_mat = zeros(n_row,n_col); + +posx = zeros(n_row,n_col); +posy = zeros(n_row,n_col); + +for i=n_row:-1:1, + for j = n_col:-1:1, + posx(i,j) = gap_x+(j-1)*(x_size+gap_x); + posy(i,j) = fig_size_y - i*(gap_x+y_size); + end; +end; + +for i=n_row:-1:1, + for j = n_col:-1:1, + if ~isempty(cell_list{i,j}), + if ~isempty(cell_list{i,j}{1}) & ~isempty(cell_list{i,j}{2}), + h_mat(i,j) = uicontrol('Parent',fig_number, ... + 'Units','points', ... + 'Callback',cell_list{i,j}{2}, ... + 'ListboxTop',0, ... + 'Position',[posx(i,j) posy(i,j) x_size y_size], ... + 'String',cell_list{i,j}{1}, ... + 'fontsize',font_size,... + 'fontname',font_name,... + 'Tag','Pushbutton1'); + end; + end; + end; +end; + +%------ END PROTECTED REGION ----------------% diff --git a/src/g_calib/stereo_triangulation.m b/src/g_calib/stereo_triangulation.m new file mode 100755 index 0000000000000000000000000000000000000000..8b3f351e4f0fd5213145162edb0c00cb0609d8da --- /dev/null +++ b/src/g_calib/stereo_triangulation.m @@ -0,0 +1,73 @@ +function [XL,XR] = stereo_triangulation(xL,xR,om,T,fc_left,cc_left,kc_left,alpha_c_left,fc_right,cc_right,kc_right,alpha_c_right), + +% [XL,XR] = stereo_triangulation(xL,xR,om,T,fc_left,cc_left,kc_left,alpha_c_left,fc_right,cc_right,kc_right,alpha_c_right), +% +% Function that computes the position of a set on N points given the left and right image projections. +% The cameras are assumed to be calibrated, intrinsically, and extrinsically. +% +% Input: +% xL: 2xN matrix of pixel coordinates in the left image +% xR: 2xN matrix of pixel coordinates in the right image +% om,T: rotation vector and translation vector between right and left cameras (output of stereo calibration) +% fc_left,cc_left,...: intrinsic parameters of the left camera (output of stereo calibration) +% fc_right,cc_right,...: intrinsic parameters of the right camera (output of stereo calibration) +% +% Output: +% +% XL: 3xN matrix of coordinates of the points in the left camera reference frame +% XR: 3xN matrix of coordinates of the points in the right camera reference frame +% +% Note: XR and XL are related to each other through the rigid motion equation: XR = R * XL + T, where R = rodrigues(om) +% For more information, visit http://www.vision.caltech.edu/bouguetj/calib_doc/htmls/example5.html +% +% +% (c) Jean-Yves Bouguet - Intel Corporation - April 9th, 2003 + + + +%--- Normalize the image projection according to the intrinsic parameters of the left and right cameras +xt = normalize_pixel(xL,fc_left,cc_left,kc_left,alpha_c_left); +xtt = normalize_pixel(xR,fc_right,cc_right,kc_right,alpha_c_right); + +%--- Extend the normalized projections in homogeneous coordinates +xt = [xt;ones(1,size(xt,2))]; +xtt = [xtt;ones(1,size(xtt,2))]; + +%--- Number of points: +N = size(xt,2); + +%--- Rotation matrix corresponding to the rigid motion between left and right cameras: +R = rodrigues(om); + + +%--- Triangulation of the rays in 3D space: + +u = R * xt; + +n_xt2 = dot(xt,xt); +n_xtt2 = dot(xtt,xtt); + +T_vect = repmat(T, [1 N]); + +DD = n_xt2 .* n_xtt2 - dot(u,xtt).^2; + +dot_uT = dot(u,T_vect); +dot_xttT = dot(xtt,T_vect); +dot_xttu = dot(u,xtt); + +NN1 = dot_xttu.*dot_xttT - n_xtt2 .* dot_uT; +NN2 = n_xt2.*dot_xttT - dot_uT.*dot_xttu; + +Zt = NN1./DD; +Ztt = NN2./DD; + +X1 = xt .* repmat(Zt,[3 1]); +X2 = R'*(xtt.*repmat(Ztt,[3,1]) - T_vect); + + +%--- Left coordinates: +XL = 1/2 * (X1 + X2); + +%--- Right coordinates: +XR = R*XL + T_vect; + diff --git a/src/g_calib/undistort_image.m b/src/g_calib/undistort_image.m new file mode 100755 index 0000000000000000000000000000000000000000..b0029f7980e7a4a3505337dc185a29cd931dda15 --- /dev/null +++ b/src/g_calib/undistort_image.m @@ -0,0 +1,206 @@ +%%% INPUT THE IMAGE FILE NAME: + +if ~exist('fc')|~exist('cc')|~exist('kc')|~exist('alpha_c'), + fprintf(1,'No intrinsic camera parameters available.\n'); + return; +end; + +KK = [fc(1) alpha_c*fc(1) cc(1);0 fc(2) cc(2) ; 0 0 1]; + +%%% Compute the new KK matrix to fit as much data in the image (in order to +%%% accomodate large distortions: +r2_extreme = (nx^2/(4*fc(1)^2) + ny^2/(4*fc(2)^2)); +dist_amount = 1; %(1+kc(1)*r2_extreme + kc(2)*r2_extreme^2); +fc_new = dist_amount * fc; + +KK_new = [fc_new(1) alpha_c*fc_new(1) cc(1);0 fc_new(2) cc(2) ; 0 0 1]; + +disp('Program that undistorts images'); +disp('The intrinsic camera parameters are assumed to be known (previously computed)'); + +fprintf(1,'\n'); + +quest = input('Do you want to undistort all the calibration images ([],0) or a new image (1)? '); + +if isempty(quest), + quest = 0; +end; + +if ~quest, + + if n_ima == 0, + fprintf(1,'No image data available\n'); + return; + end; + + if ~exist(['I_' num2str(ind_active(1))]), + ima_read_calib; + end; + + check_active_images; + + format_image2 = format_image; + if format_image2(1) == 'j', + format_image2 = 'bmp'; + end; + + for kk = 1:n_ima, + + if exist(['I_' num2str(kk)]), + + eval(['I = I_' num2str(kk) ';']); + [I2] = rect(I,eye(3),fc,cc,kc,KK_new); + + if ~type_numbering, + number_ext = num2str(image_numbers(kk)); + else + number_ext = sprintf(['%.' num2str(N_slots) 'd'],image_numbers(kk)); + end; + + ima_name2 = [calib_name '_rect' number_ext '.' format_image2]; + + fprintf(1,['Saving undistorted image under ' ima_name2 '...\n']); + + + if format_image2(1) == 'p', + if format_image2(2) == 'p', + saveppm(ima_name2,uint8(round(I2))); + else + savepgm(ima_name2,uint8(round(I2))); + end; + else + if format_image2(1) == 'r', + writeras(ima_name2,round(I2),gray(256)); + else + imwrite(uint8(round(I2)),gray(256),ima_name2,format_image2); + end; + end; + + + end; + + end; + + fprintf(1,'done\n'); + +else + + dir; + fprintf(1,'\n'); + + image_name = input('Image name (full name without extension): ','s'); + + format_image2 = '0'; + + while format_image2 == '0', + + format_image2 = input('Image format: ([]=''r''=''ras'', ''b''=''bmp'', ''t''=''tif'', ''p''=''pgm'', ''j''=''jpg'', ''m''=''ppm'') ','s'); + + if isempty(format_image2), + format_image2 = 'ras'; + end; + + if lower(format_image2(1)) == 'm', + format_image2 = 'ppm'; + else + if lower(format_image2(1)) == 'b', + format_image2 = 'bmp'; + else + if lower(format_image2(1)) == 't', + format_image2 = 'tif'; + else + if lower(format_image2(1)) == 'p', + format_image2 = 'pgm'; + else + if lower(format_image2(1)) == 'j', + format_image2 = 'jpg'; + else + if lower(format_image2(1)) == 'r', + format_image2 = 'ras'; + else + disp('Invalid image format'); + format_image2 = '0'; % Ask for format once again + end; + end; + end; + end; + end; + end; + end; + + ima_name = [image_name '.' format_image2]; + + + %%% READ IN IMAGE: + + if format_image2(1) == 'p', + if format_image2(2) == 'p', + I = double(loadppm(ima_name)); + else + I = double(loadpgm(ima_name)); + end; + else + if format_image2(1) == 'r', + I = readras(ima_name); + else + I = double(imread(ima_name)); + end; + end; + + if size(I,3)>1, + I = I(:,:,2); + end; + + + if (size(I,1)>ny)|(size(I,2)>nx), + I = I(1:ny,1:nx); + end; + + + %% SHOW THE ORIGINAL IMAGE: + + figure(2); + image(I); + colormap(gray(256)); + title('Original image (with distortion) - Stored in array I'); + drawnow; + + + %% UNDISTORT THE IMAGE: + + fprintf(1,'Computing the undistorted image...') + + [I2] = rect(I,eye(3),fc,cc,kc,alpha_c,KK_new); + + fprintf(1,'done\n'); + + figure(3); + image(I2); + colormap(gray(256)); + title('Undistorted image - Stored in array I2'); + drawnow; + + + %% SAVE THE IMAGE IN FILE: + + ima_name2 = [image_name '_rect.' format_image2]; + + fprintf(1,['Saving undistorted image under ' ima_name2 '...']); + + if format_image2(1) == 'p', + if format_image2(2) == 'p', + saveppm(ima_name2,uint8(round(I2))); + else + savepgm(ima_name2,uint8(round(I2))); + end; + else + if format_image2(1) == 'r', + writeras(ima_name2,round(I2),gray(256)); + else + imwrite(uint8(round(I2)),gray(256),ima_name2,format_image2); + end; + end; + + fprintf(1,'done\n'); + +end; diff --git a/src/g_calib/undistort_image_color.m b/src/g_calib/undistort_image_color.m new file mode 100755 index 0000000000000000000000000000000000000000..570b1cc5c000ec0e7c91501ddf6a57e1b79d5fc1 --- /dev/null +++ b/src/g_calib/undistort_image_color.m @@ -0,0 +1,174 @@ +%%% INPUT THE IMAGE FILE NAME: + +if ~exist('fc')|~exist('cc')|~exist('kc')|~exist('alpha_c') + fprintf(1,'No intrinsic camera parameters available.\n'); + return +end + +KK = [fc(1) alpha_c*fc(1) cc(1);0 fc(2) cc(2) ; 0 0 1]; + +disp('Program that corrects distorted images'); +disp('The intrinsic camera parameters are assumed to be known (previously computed)'); + +fprintf(1,'\n') + +quest = input('Do you want to undistort all the calibration images ([],0) or a new image (1)? '); + +if isempty(quest) + quest = 0; +end + +if ~quest + if ~exist(['I_' num2str(ind_active(1))]) + ima_read_calib; + end + + check_active_images; + + format_image2 = format_image; + if format_image2(1) == 'j' + format_image2 = 'bmp'; + end + + for kk = 1:n_ima + if exist(['I_' num2str(kk)]) + eval(['I = I_' num2str(kk) ';']); + [I2] = rect(I,eye(3),fc,cc,kc,KK); + + if ~type_numbering + number_ext = num2str(image_numbers(kk)); + else + number_ext = sprintf(['%.' num2str(N_slots) 'd'],image_numbers(kk)); + end + + ima_name2 = [calib_name '_rect' number_ext '.' format_image2]; + fprintf(1,['Saving undistorted image under ' ima_name2 '...\n']) + + if format_image2(1) == 'p' + if format_image2(2) == 'p' + saveppm(ima_name2,uint8(round(I2))); + else + savepgm(ima_name2,uint8(round(I2))); + end + else + if format_image2(1) == 'r' + writeras(ima_name2,round(I2),gray(256)); + else + imwrite(uint8(round(I2)),gray(256),ima_name2,format_image2); + end + end + end + end + + fprintf(1,'done\n') +else + dir + fprintf(1,'\n') + + image_name = input('Image name (full name without extension): ','s'); + + format_image2 = '0'; + + while format_image2 == '0' + format_image2 = input('Image format ([]=''r''=''ras'', ''b''=''bmp'', ''t''=''tif'', ''p''=''pgm'', ''j''=''jpg'', ''m''=''ppm''): ','s'); + + if isempty(format_image2) + format_image2 = 'ras'; + end + + if lower(format_image2(1)) == 'm' + format_image2 = 'ppm'; + else + if lower(format_image2(1)) == 'b' + format_image2 = 'bmp'; + else + if lower(format_image2(1)) == 't' + format_image2 = 'tif'; + else + if lower(format_image2(1)) == 'p' + format_image2 = 'pgm'; + else + if lower(format_image2(1)) == 'j' + format_image2 = 'jpg'; + else + if lower(format_image2(1)) == 'r' + format_image2 = 'ras'; + else + disp('Invalid image format'); + format_image2 = '0'; % Ask for format once again + end + end + end + end + end + end + end + + ima_name = [image_name '.' format_image2]; + + + %%% READ IN IMAGE: + if format_image2(1) == 'p' + if format_image2(2) == 'p' + I = double(loadppm(ima_name)); + else + I = double(loadpgm(ima_name)); + end + else + if format_image2(1) == 'r' + I = readras(ima_name); + else + I = double(imread(ima_name)); + end + end + + if (size(I,1)>ny)|(size(I,2)>nx) + I = I(1:ny,1:nx); + end + + %% SHOW THE ORIGINAL IMAGE: + figure(2); + image(uint8(I)); + title('Original image (with distortion) - Stored in array I') + drawnow; + + %% UNDISTORT THE IMAGE: + fprintf(1,'Computing the undistorted image...') + + [Ipart_1] = rect(I(:,:,1),eye(3),fc,cc,kc,alpha_c,KK); + [Ipart_2] = rect(I(:,:,2),eye(3),fc,cc,kc,alpha_c,KK); + [Ipart_3] = rect(I(:,:,3),eye(3),fc,cc,kc,alpha_c,KK); + + I2 = ones(ny, nx,3); + I2(:,:,1) = Ipart_1; + I2(:,:,2) = Ipart_2; + I2(:,:,3) = Ipart_3; + + fprintf(1,'done\n') + + figure(3); + image(uint8(I2)); + title('Undistorted image - Stored in array I2') + drawnow; + + %% SAVE THE IMAGE IN FILE: + ima_name2 = [image_name '_rect_color.' format_image2]; + + fprintf(1,['Saving undistorted image under ' ima_name2 '...']) + + if format_image2(1) == 'p' + if format_image2(2) == 'p' + saveppm(ima_name2,uint8(round(I2))); + else + savepgm(ima_name2,uint8(round(I2))); + end + else + if format_image2(1) == 'r' + writeras(ima_name2,round(I2),gray(256)); + else + imwrite(uint8(round(I2)),ima_name2,format_image2); + end + end + + fprintf(1,'done\n') +end \ No newline at end of file diff --git a/src/g_calib/undistort_image_no_read.m b/src/g_calib/undistort_image_no_read.m new file mode 100755 index 0000000000000000000000000000000000000000..23aaa46f21a5c4e84e19be370751724ca1e965e0 --- /dev/null +++ b/src/g_calib/undistort_image_no_read.m @@ -0,0 +1,232 @@ +%%% INPUT THE IMAGE FILE NAME: + +if ~exist('fc')|~exist('cc')|~exist('kc')|~exist('alpha_c'), + fprintf(1,'No intrinsic camera parameters available.\n'); + return; +end; + +KK = [fc(1) alpha_c*fc(1) cc(1);0 fc(2) cc(2) ; 0 0 1]; + +disp('Program that undistorts images'); +disp('The intrinsic camera parameters are assumed to be known (previously computed)'); + +fprintf(1,'\n'); + +quest = input('Do you want to undistort all the calibration images ([],0) or a new image (1)? '); + +if isempty(quest), + quest = 0; +end; + +if ~quest, + + %if ~exist(['I_' num2str(ind_active(1))]), + %ima_read_calib; + %end; + + if n_ima == 0, + fprintf(1,'No image data available\n'); + return; + end; + + check_active_images; + + format_image2 = format_image; + if format_image2(1) == 'j', + format_image2 = 'bmp'; + end; + + for kk = 1:n_ima, + + + if ~type_numbering, + number_ext = num2str(image_numbers(kk)); + else + number_ext = sprintf(['%.' num2str(N_slots) 'd'],image_numbers(kk)); + end; + + ima_name = [calib_name number_ext '.' format_image]; + + + if ~exist(ima_name), + + fprintf(1,'Image %s not found!!!\n',ima_name); + + else + + fprintf(1,'Loading image %s...\n',ima_name); + + if format_image(1) == 'p', + if format_image(2) == 'p', + I = double(loadppm(ima_name)); + else + I = double(loadpgm(ima_name)); + end; + else + if format_image(1) == 'r', + I = readras(ima_name); + else + I = double(imread(ima_name)); + end; + end; + + + if size(I,3)>1, + I = 0.299 * I(:,:,1) + 0.5870 * I(:,:,2) + 0.114 * I(:,:,3); + end; + + [I2] = rect(I,eye(3),fc,cc,kc,KK); + + if ~type_numbering, + number_ext = num2str(image_numbers(kk)); + else + number_ext = sprintf(['%.' num2str(N_slots) 'd'],image_numbers(kk)); + end; + + ima_name2 = [calib_name '_rect' number_ext '.' format_image2]; + + fprintf(1,['Saving undistorted image under ' ima_name2 '...\n']); + + + if format_image2(1) == 'p', + if format_image2(2) == 'p', + saveppm(ima_name2,uint8(round(I2))); + else + savepgm(ima_name2,uint8(round(I2))); + end; + else + if format_image2(1) == 'r', + writeras(ima_name2,round(I2),gray(256)); + else + imwrite(uint8(round(I2)),gray(256),ima_name2,format_image2); + end; + end; + + + end; + + end; + + fprintf(1,'done\n'); + +else + + dir; + fprintf(1,'\n'); + + image_name = input('Image name (full name without extension): ','s'); + + format_image2 = '0'; + + while format_image2 == '0', + + format_image2 = input('Image format: ([]=''r''=''ras'', ''b''=''bmp'', ''t''=''tif'', ''p''=''pgm'', ''j''=''jpg'', ''m''=''ppm'') ','s'); + + if isempty(format_image2), + format_image2 = 'ras'; + end; + + if lower(format_image2(1)) == 'm', + format_image2 = 'ppm'; + else + if lower(format_image2(1)) == 'b', + format_image2 = 'bmp'; + else + if lower(format_image2(1)) == 't', + format_image2 = 'tif'; + else + if lower(format_image2(1)) == 'p', + format_image2 = 'pgm'; + else + if lower(format_image2(1)) == 'j', + format_image2 = 'jpg'; + else + if lower(format_image2(1)) == 'r', + format_image2 = 'ras'; + else + disp('Invalid image format'); + format_image2 = '0'; % Ask for format once again + end; + end; + end; + end; + end; + end; + end; + + ima_name = [image_name '.' format_image2]; + + + %%% READ IN IMAGE: + + if format_image2(1) == 'p', + if format_image2(2) == 'p', + I = double(loadppm(ima_name)); + else + I = double(loadpgm(ima_name)); + end; + else + if format_image2(1) == 'r', + I = readras(ima_name); + else + I = double(imread(ima_name)); + end; + end; + + if size(I,3)>1, + I = I(:,:,2); + end; + + + if (size(I,1)>ny)|(size(I,2)>nx), + I = I(1:ny,1:nx); + end; + + + %% SHOW THE ORIGINAL IMAGE: + + figure(2); + image(I); + colormap(gray(256)); + title('Original image (with distortion) - Stored in array I'); + drawnow; + + + %% UNDISTORT THE IMAGE: + + fprintf(1,'Computing the undistorted image...') + + [I2] = rect(I,eye(3),fc,cc,kc,alpha_c,KK); + + fprintf(1,'done\n'); + + figure(3); + image(I2); + colormap(gray(256)); + title('Undistorted image - Stored in array I2'); + drawnow; + + + %% SAVE THE IMAGE IN FILE: + + ima_name2 = [image_name '_rect.' format_image2]; + + fprintf(1,['Saving undistorted image under ' ima_name2 '...']); + + if format_image2(1) == 'p', + if format_image2(2) == 'p', + saveppm(ima_name2,uint8(round(I2))); + else + savepgm(ima_name2,uint8(round(I2))); + end; + else + if format_image2(1) == 'r', + writeras(ima_name2,round(I2),gray(256)); + else + imwrite(uint8(round(I2)),gray(256),ima_name2,format_image2); + end; + end; + + fprintf(1,'done\n'); + +end; diff --git a/src/g_calib/undistort_sequence.m b/src/g_calib/undistort_sequence.m new file mode 100755 index 0000000000000000000000000000000000000000..f11787b693a33a57390020eeea9d5d5fb4fd6de7 --- /dev/null +++ b/src/g_calib/undistort_sequence.m @@ -0,0 +1,161 @@ +%%% INPUT THE IMAGE FILE NAME: + +graphout = 0; + +if ~exist('fc')|~exist('cc')|~exist('kc')|~exist('alpha_c'), + fprintf(1,'No intrinsic camera parameters available. Maybe, need to load Calib_Results.mat\n'); + return; +end; + +KK = [fc(1) alpha_c*fc(1) cc(1);0 fc(2) cc(2) ; 0 0 1]; + +disp('Program that undistorts a whole sequence of images (works with bmp only so far... needs some debugging)'); +disp('The intrinsic camera parameters are assumed to be known (previously computed)'); +disp('After undistortion, the intrinsic parameters fc, cc, alpha_c remain unchanged. The distortion coefficient vector kc is zero'); + +dir; + +fprintf(1,'\n'); + +seq_name = input('Basename of sequence images (without number nor suffix): ','s'); + +format_image_seq = '0'; + +while format_image_seq == '0', + format_image_seq = input('Image format: ([]=''r''=''ras'', ''b''=''bmp'', ''t''=''tif'', ''p''=''pgm'', ''j''=''jpg'', ''m''=''ppm'') ','s'); + if isempty(format_image_seq), + format_image_seq = 'ras'; + else + if lower(format_image_seq(1)) == 'm', + format_image_seq = 'ppm'; + else + if lower(format_image_seq(1)) == 'b', + format_image_seq = 'bmp'; + else + if lower(format_image_seq(1)) == 't', + format_image_seq = 'tif'; + else + if lower(format_image_seq(1)) == 'p', + format_image_seq = 'pgm'; + else + if lower(format_image_seq(1)) == 'j', + format_image_seq = 'jpg'; + else + if lower(format_image_seq(1)) == 'r', + format_image_seq = 'ras'; + else + disp('Invalid image format'); + format_image_seq = '0'; % Ask for format once again + end; + end; + end; + end; + end; + end; + end; +end; + + +ima_sequence = dir( [ seq_name '*.' format_image_seq]); + +if isempty(ima_sequence), + fprintf(1,'No image found\n'); + return; +end; + +ima_name = ima_sequence(1).name; +if format_image_seq(1) == 'p', + if format_image_seq(2) == 'p', + I = double(loadppm(ima_name)); + else + I = double(loadpgm(ima_name)); + end; +else + if format_image_seq(1) == 'r', + I = readras(ima_name); + else + I = double(imread(ima_name)); + end; +end; + +[ny,nx,nc] = size(I); + + +% Pre-compute the necessary indices and blending coefficients to enable quick rectification: +[Irec_junk,ind_new,ind_1,ind_2,ind_3,ind_4,a1,a2,a3,a4] = rect_index(zeros(ny,nx),eye(3),fc,cc,kc,alpha_c,KK); + + +n_seq = length(ima_sequence); + + +for kk = 1:n_seq, + + ima_name = ima_sequence(kk).name; + + fprintf(1,'Loading original image %s...',ima_name); + + %%% READ IN IMAGE: + + if format_image_seq(1) == 'p', + if format_image_seq(2) == 'p', + I = double(loadppm(ima_name)); + else + I = double(loadpgm(ima_name)); + end; + else + if format_image_seq(1) == 'r', + I = readras(ima_name); + else + I = double(imread(ima_name)); + end; + end; + + [ny,nx,nc] = size(I); + + if graphout, + figure(2); + image(uint8(I)); + drawnow; + end; + + I2 = zeros(ny,nx,nc); + + for ii = 1:nc, + + Iii = I(:,:,ii); + I2ii = zeros(ny,nx); + + I2ii(ind_new) = uint8(a1 .* Iii(ind_1) + a2 .* Iii(ind_2) + a3 .* Iii(ind_3) + a4 .* Iii(ind_4)); + + I2(:,:,ii) = I2ii; + + end; + + I2 = uint8(I2); + + if graphout, + figure(3); + image(I2); + drawnow; + end; + + ima_name2 = ['undist_' ima_name]; + + fprintf(1,'Saving undistorted image under %s...\n',ima_name2); + + if format_image_seq(1) == 'p', + if format_image_seq(2) == 'p', + saveppm(ima_name2,I2); + else + savepgm(ima_name2,I2); + end; + else + if format_image_seq(1) == 'r', + writeras(ima_name2,I2,gray(256)); + else + imwrite(I2,ima_name2,format_image_seq); + end; + end; + + +end; diff --git a/src/g_calib/visualize_distortions.m b/src/g_calib/visualize_distortions.m new file mode 100755 index 0000000000000000000000000000000000000000..ac515d0e59ac72a28e6e3e6006da995467c17bae --- /dev/null +++ b/src/g_calib/visualize_distortions.m @@ -0,0 +1,217 @@ +% visualize_distortions +% +% +% A script to run in conjunction with calib_gui in TOOLBOX_calib to plot +% the distortion models. +% +% This is a slightly modified version of the script plot_CCT_distortion.m written by Mr. Oshel +% Thank you Mr. Oshel for your contribution! + + +[mx,my] = meshgrid(0:nx/20:(nx-1),0:ny/20:(ny-1)); +[nnx,nny]=size(mx); +px=reshape(mx',nnx*nny,1); +py=reshape(my',nnx*nny,1); +kk_new=[fc(1) alpha_c*fc(1) cc(1);0 fc(2) cc(2);0 0 1]; +rays=inv(kk_new)*[px';py';ones(1,length(px))]; +x=[rays(1,:)./rays(3,:);rays(2,:)./rays(3,:)]; + + +title2=strcat('Complete Distortion Model'); + +fh1 = 2; + +%if ishandle(fh1), +% close(fh1); +%end; +figure(fh1); clf; +xd=apply_distortion(x,kc); +px2=fc(1)*(xd(1,:)+alpha_c*xd(2,:))+cc(1); +py2=fc(2)*xd(2,:)+cc(2); +dx=px2'-px; +dy=py2'-py; +Q=quiver(px+1,py+1,dx,dy); +hold on; +plot(cc(1)+1,cc(2)+1,'o'); +plot((nx-1)/2+1,(ny-1)/2+1,'x'); +dr=reshape(sqrt((dx.*dx)+(dy.*dy)),nny,nnx)'; +[C,h]=contour(mx,my,dr,'k'); +clabel(C,h); +Mean=mean(mean(dr)); +Max=max(max(dr)); +title(title2); + +axis ij; +axis([1 nx 1 ny]) +axis equal; +axis tight; + +position=get(gca,'Position'); +shr = 0.9; +position(1)=position(1)+position(3)*((1-shr)/2); +position(2)=position(2)+position(4)*(1-shr)+0.03; +position(3:4)=position(3:4)*shr; +set(gca,'position',position); +set(gca,'fontsize',8,'fontname','clean') + +gh = gca; + +line1=sprintf('Principal Point = (%0.6g, %0.6g)',cc(1),cc(2)); +line2=sprintf('Focal Length = (%0.6g, %0.6g)',fc(1),fc(2)); +line3=sprintf('Radial coefficients = (%0.4g, %0.4g, %0.4g)',kc(1),kc(2),kc(5)); +line4=sprintf('Tangential coefficients = (%0.4g, %0.4g)',kc(3),kc(4)); +line5=sprintf('+/- [%0.4g, %0.4g]',cc_error(1),cc_error(2)); +line6=sprintf('+/- [%0.4g, %0.4g]',fc_error(1),fc_error(2)); +line7=sprintf('+/- [%0.4g, %0.4g, %0.4g]',kc_error(1),kc_error(2),kc_error(5)); +line8=sprintf('+/- [%0.4g, %0.4g]',kc_error(3),kc_error(4)); +line9=sprintf('Pixel error = [%0.4g, %0.4g]',err_std(1),err_std(2)); +line10=sprintf('Skew = %0.4g',alpha_c); +line11=sprintf('+/- %0.4g',alpha_c_error); + + +axes('position',[0 0 1 1],'visible','off'); +th=text(0.11,0,{line9,line2,line1,line10,line3,line4},'horizontalalignment','left','verticalalignment','bottom','fontsize',8,'fontname','clean'); +th2=text(0.9,0.,{line6,line5,line11,line7,line8},'horizontalalignment','right','verticalalignment','bottom','fontsize',8,'fontname','clean'); +%set(th,'FontName','fixed'); +axes(gh); + +set(fh1,'color',[1,1,1]); + +hold off; + + + + + +title2=strcat('Tangential Component of the Distortion Model'); + +fh2 = 3; + +%if ishandle(fh2), +% close(fh2); +%end; +figure(fh2); clf; +xd=apply_distortion(x,[0 0 kc(3) kc(4) 0]); +px2=fc(1)*(xd(1,:)+alpha_c*xd(2,:))+cc(1); +py2=fc(2)*xd(2,:)+cc(2); +dx=px2'-px; +dy=py2'-py; +Q=quiver(px+1,py+1,dx,dy); +hold on; +plot(cc(1)+1,cc(2)+1,'o'); +plot((nx-1)/2+1,(ny-1)/2+1,'x'); +dr=reshape(sqrt((dx.*dx)+(dy.*dy)),nny,nnx)'; +[C,h]=contour(mx,my,dr,'k'); +clabel(C,h); +Mean=mean(mean(dr)); +Max=max(max(dr)); +title(title2); + +axis ij; +axis([1 nx 1 ny]) +axis equal; +axis tight; + +position=get(gca,'Position'); +shr = 0.9; +position(1)=position(1)+position(3)*((1-shr)/2); +position(2)=position(2)+position(4)*(1-shr)+0.03; +position(3:4)=position(3:4)*shr; +set(gca,'position',position); +set(gca,'fontsize',8,'fontname','clean') + +gh = gca; + +line1=sprintf('Principal Point = (%0.6g, %0.6g)',cc(1),cc(2)); +line2=sprintf('Focal Length = (%0.6g, %0.6g)',fc(1),fc(2)); +line3=sprintf('Radial coefficients = (%0.4g, %0.4g, %0.4g)',kc(1),kc(2),kc(5)); +line4=sprintf('Tangential coefficients = (%0.4g, %0.4g)',kc(3),kc(4)); +line5=sprintf('+/- [%0.4g, %0.4g]',cc_error(1),cc_error(2)); +line6=sprintf('+/- [%0.4g, %0.4g]',fc_error(1),fc_error(2)); +line7=sprintf('+/- [%0.4g, %0.4g, %0.4g]',kc_error(1),kc_error(2),kc_error(5)); +line8=sprintf('+/- [%0.4g, %0.4g]',kc_error(3),kc_error(4)); +line9=sprintf('Pixel error = [%0.4g, %0.4g]',err_std(1),err_std(2)); +line10=sprintf('Skew = %0.4g',alpha_c); +line11=sprintf('+/- %0.4g',alpha_c_error); + + +axes('position',[0 0 1 1],'visible','off'); +th=text(0.11,0,{line9,line2,line1,line10,line3,line4},'horizontalalignment','left','verticalalignment','bottom','fontsize',8,'fontname','clean'); +th2=text(0.9,0.,{line6,line5,line11,line7,line8},'horizontalalignment','right','verticalalignment','bottom','fontsize',8,'fontname','clean'); +%set(th,'FontName','fixed'); +axes(gh); + +set(fh2,'color',[1,1,1]); + +hold off; + + + + + + + + +title2=strcat('Radial Component of the Distortion Model'); + +fh3 = 4; + +%if ishandle(fh3), +% close(fh3); +%end; +figure(fh3); clf; +xd=apply_distortion(x,[kc(1) kc(2) 0 0 kc(5)]); +px2=fc(1)*(xd(1,:)+alpha_c*xd(2,:))+cc(1); +py2=fc(2)*xd(2,:)+cc(2); +dx=px2'-px; +dy=py2'-py; +Q=quiver(px+1,py+1,dx,dy); +hold on; +plot(cc(1)+1,cc(2)+1,'o'); +plot((nx-1)/2+1,(ny-1)/2+1,'x'); +dr=reshape(sqrt((dx.*dx)+(dy.*dy)),nny,nnx)'; +[C,h]=contour(mx,my,dr,'k'); +clabel(C,h); +Mean=mean(mean(dr)); +Max=max(max(dr)); +title(title2); + +axis ij; +axis([1 nx 1 ny]) +axis equal; +axis tight; + +position=get(gca,'Position'); +shr = 0.9; +position(1)=position(1)+position(3)*((1-shr)/2); +position(2)=position(2)+position(4)*(1-shr)+0.03; +position(3:4)=position(3:4)*shr; +set(gca,'position',position); +set(gca,'fontsize',8,'fontname','clean') + +gh = gca; + +line1=sprintf('Principal Point = (%0.6g, %0.6g)',cc(1),cc(2)); +line2=sprintf('Focal Length = (%0.6g, %0.6g)',fc(1),fc(2)); +line3=sprintf('Radial coefficients = (%0.4g, %0.4g, %0.4g)',kc(1),kc(2),kc(5)); +line4=sprintf('Tangential coefficients = (%0.4g, %0.4g)',kc(3),kc(4)); +line5=sprintf('+/- [%0.4g, %0.4g]',cc_error(1),cc_error(2)); +line6=sprintf('+/- [%0.4g, %0.4g]',fc_error(1),fc_error(2)); +line7=sprintf('+/- [%0.4g, %0.4g, %0.4g]',kc_error(1),kc_error(2),kc_error(5)); +line8=sprintf('+/- [%0.4g, %0.4g]',kc_error(3),kc_error(4)); +line9=sprintf('Pixel error = [%0.4g, %0.4g]',err_std(1),err_std(2)); +line10=sprintf('Skew = %0.4g',alpha_c); +line11=sprintf('+/- %0.4g',alpha_c_error); + + +axes('position',[0 0 1 1],'visible','off'); +th=text(0.11,0,{line9,line2,line1,line10,line3,line4},'horizontalalignment','left','verticalalignment','bottom','fontsize',8,'fontname','clean'); +th2=text(0.9,0.,{line6,line5,line11,line7,line8},'horizontalalignment','right','verticalalignment','bottom','fontsize',8,'fontname','clean'); +%set(th,'FontName','fixed'); +axes(gh); + +set(fh3,'color',[1,1,1]); + +hold off; + +figure(fh1); diff --git a/src/g_calib/willson_convert.m b/src/g_calib/willson_convert.m new file mode 100755 index 0000000000000000000000000000000000000000..952718163958486bacea82fffd1612ce4a97668a --- /dev/null +++ b/src/g_calib/willson_convert.m @@ -0,0 +1,95 @@ +function [fc,cc,kc,alpha_c,Rc,Tc,omc,nx,ny,x_dist,xd] = willson_convert(Ncx,Nfx,dx,dy,dpx,dpy,Cx,Cy,sx,f,kappa1,Tx,Ty,Tz,Rx,Ry,Rz,p1,p2); + +%Conversion from Reg Willson's calibration format to my format + +% Conversion: + +% Focal length: +fc = [sx/dpx ; 1/dpy]*f; + +% Principal point; +cc = [Cx;Cy]; + +% Skew: +alpha_c = 0; + +% Extrinsic parameters: +Rx = rodrigues([Rx;0;0]); +Ry = rodrigues([0;Ry;0]); +Rz = rodrigues([0;0;Rz]); + +Rc = Rz * Ry * Rx; + +omc = rodrigues(Rc); + +Tc = [Tx;Ty;Tz]; + + +% More tricky: Take care of the distorsion: + +Nfy = round(Nfx * 3/4); + +nx = Nfx; +ny = Nfy; + +% Select a set of DISTORTED coordinates uniformely distributed across the image: + +[xp_dist,yp_dist] = meshgrid(0:Nfx-1,0:Nfy); + +xp_dist = xp_dist(:)'; +yp_dist = yp_dist(:)'; + + +% Apply UNDISTORTION according to Willson: + +xp_sensor_dist = dpx*(xp_dist - Cx)/sx; +yp_sensor_dist = dpy*(yp_dist - Cy); + +dist_fact = 1 + kappa1*(xp_sensor_dist.^2 + yp_sensor_dist.^2); + +xp_sensor = xp_sensor_dist .* dist_fact; +yp_sensor = yp_sensor_dist .* dist_fact; + +xp = xp_sensor * sx / dpx + Cx; +yp = yp_sensor / dpy + Cy; + +ind= find((xp > 0) & (xp < Nfx-1) & (yp > 0) & (yp < Nfy-1)); + +xp = xp(ind); +yp = yp(ind); +xp_dist = xp_dist(ind); +yp_dist = yp_dist(ind); + + +% Now, find my own set of parameters: + +x_dist = [(xp_dist - cc(1))/fc(1);(yp_dist - cc(2))/fc(2)]; +x_dist(1,:) = x_dist(1,:) - alpha_c * x_dist(2,:); + +x = [(xp - cc(1))/fc(1);(yp - cc(2))/fc(2)]; +x(1,:) = x(1,:) - alpha_c * x(2,:); + +k = [0;0;0;0;0]; + +for kk = 1:5, + + [xd,dxddk] = apply_distortion(x,k); + + err = x_dist - xd; + + %norm(err) + + k_step = inv(dxddk'*dxddk)*(dxddk')*err(:); + + k = k + k_step; %inv(dxddk'*dxddk)*(dxddk')*err(:); + + %norm(k_step)/norm(k) + + if norm(k_step)/norm(k) < 10e-10, + break; + end; + +end; + + +kc = k; diff --git a/src/g_calib/willson_read.m b/src/g_calib/willson_read.m new file mode 100755 index 0000000000000000000000000000000000000000..60e57ffee2be5177c26299b1d5e49259d95c6d92 --- /dev/null +++ b/src/g_calib/willson_read.m @@ -0,0 +1,92 @@ +% Read in Reg Willson's data file, and convert it into my data format: + +if exist('calib_file'), + if exist(calib_file)~=2, + ask = 1; + else + ask = 0; + end; +else + ask = 1; +end; + + +while ask, + + calib_file = input('Reg Willson calibration file name to convert: ','s'); + + if ~isempty(calib_file), + if exist(calib_file)~=2, + ask = 1; + else + ask = 0; + end; + else + ask = 1; + end; + + if ask, + fprintf(1,'File not found. Try again.\n'); + end; + +end; + +if exist(calib_file), + + fprintf(1,['Loading Willson calibration parameters from file ' calib_file '...\n']); + + load(calib_file); + + inddot = find(calib_file == '.'); + + if isempty(inddot), + varname = calib_file; + else + varname = calib_file(1:inddot(1)-1); + end; + + eval(['calib_params = ' varname ';']) + + Ncx = calib_params(1); + Nfx = calib_params(2); + dx = calib_params(3); + dy = calib_params(4); + dpx = calib_params(5); + dpy = calib_params(6); + Cx = calib_params(7); + Cy = calib_params(8); + sx = calib_params(9); + f = calib_params(10); + kappa1 = calib_params(11); + Tx = calib_params(12); + Ty = calib_params(13); + Tz = calib_params(14); + Rx = calib_params(15); + Ry = calib_params(16); + Rz = calib_params(17); + p1 = calib_params(18); + p2 = calib_params(19); + + fprintf(1,'Converting the calibration parameters... (may take a while due to the convertion of the distortion model)...\n'); + + % Conversion: + [fc,cc,kc,alpha_c,Rc_1,Tc_1,omc_1,nx,ny,x_dist,xd] = willson_convert(Ncx,Nfx,dx,dy,dpx,dpy,Cx,Cy,sx,f,kappa1,Tx,Ty,Tz,Rx,Ry,Rz,p1,p2); + + KK = [fc(1) alpha_c*fc(1) cc(1);0 fc(2) cc(2) ; 0 0 1]; + + n_ima = 1; + + X_1 = [NaN;NaN;NaN]; + x_1 = [NaN;NaN]; + + map = gray(256); + + fprintf(1,'done\n'); + fprintf(1,'You are now ready to save the calibration parameters by clicking on Save.\n'); + +else + + disp(['WARNING: Calibration file ' calib_file ' not found']); + +end; + diff --git a/src/g_calib/write_image.m b/src/g_calib/write_image.m new file mode 100755 index 0000000000000000000000000000000000000000..840428ec6b7bd82a2a8edffc0789803d5937e05f --- /dev/null +++ b/src/g_calib/write_image.m @@ -0,0 +1,30 @@ +function [] = write_image(I, kk , calib_name , format_image , type_numbering , image_numbers , N_slots), + +if format_image(1) == 'j', + format_image = 'bmp'; +end; + + +if ~type_numbering, + number_ext = num2str(image_numbers(kk)); +else + number_ext = sprintf(['%.' num2str(N_slots) 'd'],image_numbers(kk)); +end; + +ima_name2 = [calib_name number_ext '.' format_image]; + +fprintf(1,['Saving image under ' ima_name2 '...\n']); + +if format_image(1) == 'p', + if format_image(2) == 'p', + saveppm(ima_name2,uint8(round(I))); + else + savepgm(ima_name2,uint8(round(I))); + end; +else + if format_image(1) == 'r', + writeras(ima_name2,round(I),gray(256)); + else + imwrite(uint8(round(I)),gray(256),ima_name2,format_image); + end; +end; diff --git a/src/g_calib/writeras.m b/src/g_calib/writeras.m new file mode 100755 index 0000000000000000000000000000000000000000..c7eb7bc25fa74911331d9169362f895e0e63a7e0 --- /dev/null +++ b/src/g_calib/writeras.m @@ -0,0 +1,105 @@ +function writeras(filename, image, map); +%WRITERAS Write an image file in sun raster format. +% WRITERAS('imagefile.ras', image_matrix, map) writes a +% "sun.raster" image file. + +% Written by Thomas K. Leung 3/30/93. +% @ California Institute of Technology. + + +% PC and UNIX version of writeras - Jean-Yves Bouguet - Dec. 1998 + +dot = max(find(filename == '.')); +suffix = filename(dot+1:dot+3); + +if nargin < 3, + map = []; +end; + +if(strcmp(suffix, 'ras')) + %Write header + + fp = fopen(filename, 'wb'); + if(fp < 0) error(['Cannot open ' filename '.']), end + + [height, width] = size(image); + image = image - 1; + mapsize = size(map, 1)*size(map,2); + %fwrite(fp, ... + % [1504078485, width, height, 8, height*width, 1, 1, mapsize], ... + % 'long'); + + + zero_str = '00000000'; + + % MAGIC NUMBER: + + + fwrite(fp,89,'uchar'); + fwrite(fp,166,'uchar'); + fwrite(fp,106,'uchar'); + fwrite(fp,149,'uchar'); + + width_str = dec2hex(width); + width_str = [zero_str(1:8-length(width_str)) width_str]; + + for ii = 1:2:7, + fwrite(fp,hex2dec(width_str(ii:ii+1)),'uchar'); + end; + + + height_str = dec2hex(height); + height_str = [zero_str(1:8-length(height_str)) height_str]; + + for ii = 1:2:7, + fwrite(fp,hex2dec(height_str(ii:ii+1)),'uchar'); + end; + + fwrite(fp,0,'uchar'); + fwrite(fp,0,'uchar'); + fwrite(fp,0,'uchar'); + fwrite(fp,8,'uchar'); + + ll = height*width; + ll_str = dec2hex(ll); + ll_str = [zero_str(1:8-length(ll_str)) ll_str]; + + for ii = 1:2:7, + fwrite(fp,hex2dec(ll_str(ii:ii+1)),'uchar'); + end; + + fwrite(fp,0,'uchar'); + fwrite(fp,0,'uchar'); + fwrite(fp,0,'uchar'); + fwrite(fp,1,'uchar'); + fwrite(fp,0,'uchar'); + fwrite(fp,0,'uchar'); + fwrite(fp,0,'uchar'); + fwrite(fp,~~mapsize,'uchar'); + + mapsize_str = dec2hex(mapsize); + mapsize_str = [zero_str(1:8-length(mapsize_str)) mapsize_str]; + + %keyboard; + + for ii = 1:2:7, + fwrite(fp,hex2dec(mapsize_str(ii:ii+1)),'uchar'); + end; + + + if mapsize ~= 0 + map = min(255, fix(255*map)); + fwrite(fp, map, 'uchar'); + end + if rem(width,2) == 1 + image = [image'; zeros(1, height)]'; + top = 255 * ones(size(image)); + fwrite(fp, min(top,image)', 'uchar'); + else + top = 255 * ones(size(image)); + fwrite(fp, min(top,image)', 'uchar'); + end + fclose(fp); +else + error('Image file name must end in either ''ras'' or ''rast''.'); +end diff --git a/src/g_rect/g_dist.m b/src/g_rect/g_dist.m new file mode 100755 index 0000000000000000000000000000000000000000..8187ac05f7ecc65aa91092e89c806c56b7e1780c --- /dev/null +++ b/src/g_rect/g_dist.m @@ -0,0 +1,29 @@ +function distance = g_dist(lon1,lat1,lon2,lat2,field); + +% This function computes the distance (m) between two points on a +% Cartesian Earth given their lat-lon coordinate. +% +% If field = false then simple cartesian transformation +% +% + +dlon = lon2 - lon1; +dlat = lat2 - lat1; + +if field + + meterPerDegLat = 1852*60.0; + meterPerDegLon = meterPerDegLat * cosd(lat1); + dx = dlon*meterPerDegLon; + dy = dlat*meterPerDegLat; + +else + + dx = dlon; + dy = dlat; + +end + +distance = sqrt(dx^2 + dy^2); + +end diff --git a/src/g_rect/g_error_geofit.m b/src/g_rect/g_error_geofit.m new file mode 100755 index 0000000000000000000000000000000000000000..48664cc458b9d4b0b52614fcf63de8ae69599ee0 --- /dev/null +++ b/src/g_rect/g_error_geofit.m @@ -0,0 +1,79 @@ +function errGeoFit = g_error_geofit(cv,imgWidth,imgHeight,xp,yp,ic,jc,... + hfov,lambda,phi,H,theta,... + hfov0,lambda0,phi0,H0,theta0,... + hfovGuess,lambdaGuess,phiGuess,HGuess,thetaGuess,... + dhfov,dlambda,dphi,dH,dtheta,... + LON0,LAT0,... + i_gcp,j_gcp,lon_gcp,lat_gcp,... + theOrder,field); + + +for i=1:length(theOrder) + if theOrder(i) == 1; hfov = cv(i); end + if theOrder(i) == 2; lambda = cv(i); end + if theOrder(i) == 3; phi = cv(i); end + if theOrder(i) == 4; H = cv(i); end + if theOrder(i) == 5; theta = cv(i); end +end + +% Perform the geometrical transformation +[LON LAT] = g_pix2ll(xp,yp,imgWidth,imgHeight,ic,jc,... + hfov,lambda,phi,theta,H,LON0,LAT0,field); + + +% Calculate the error between ground control points (GCPs) +% and image control points once georectified. +% +% This error (errGeoFit) is the error associated with the geometrical fit, +% as opposed to the error associated with a polynomial fit that can be +% calculated if requested by the user. +% + +% Determine the number of CGP +ngcp = length(i_gcp); + +ngcpFinite = 0; +errGeoFit = 0.0; + +for k = 1:ngcp + + % Calculate the distance (i.e. error) between GCP and rectificed ICPs. + distance = g_dist(lon_gcp(k),lat_gcp(k),LON(k),LAT(k),field); + + % Check if the distance is finite. The distance may be NaN for some + % GCPs that may temporarily be above the horizon. Those points are + % blanked out in the function g_pix2ll. + if isfinite(distance) == 1 + errGeoFit = errGeoFit + distance^2; + ngcpFinite = ngcpFinite + 1; + else + errGeoFit = Inf; + end + +end + +% rms distance +errGeoFit = sqrt(errGeoFit/ngcpFinite); + +% Check if the parameters are within the specified uncertainties. +% If not set the error to infinity. +if abs(hfov - hfovGuess) > dhfov; errGeoFit = inf; end +if abs(lambda - lambdaGuess) > dlambda; errGeoFit = inf; end +if abs(phi - phiGuess) > dphi; errGeoFit = inf; end +if abs(H - HGuess) > dH; errGeoFit = inf; end +if abs(theta - thetaGuess) > dtheta; errGeoFit = inf; end + + +%fprintf('\n') +%fprintf('Field of view (fov): %f\n',hfov) +%fprintf('Dip angle (lambda): %f\n',lambda) +%fprintf('Tilt angle (phi): %f\n',phi) +%fprintf('Camera altitude (H): %f\n',H) +%fprintf('View angle from North (theta): %f\n',theta) +%fprintf('RMS error (m): %f\n',errGeoFit) +%fprintf('\n') + +%figure(100) +%hold on; +%plot(errGeoFit); + diff --git a/src/g_rect/g_error_polyfit.m b/src/g_rect/g_error_polyfit.m new file mode 100755 index 0000000000000000000000000000000000000000..44ea37d97776edb76955310cfaf284c36f636929 --- /dev/null +++ b/src/g_rect/g_error_polyfit.m @@ -0,0 +1,21 @@ +function err=g_error_polyfit(cv,lonlon,latlat,err_ll,order); +%function err=g_error_polyfit(cv,lonlon,latlat,err_ll,order); + +n_gcp=length(err_ll); +err=0; + +for k=1:n_gcp + + if order == 1 + + efit = cv(1)*lonlon(k)+cv(2)*latlat(k)+cv(3); + + elseif order == 2 + + efit = cv(1)*lonlon(k)^2+cv(2)*latlat(k)^2+cv(3)*lonlon(k)*latlat(k)+cv(4)*lonlon(k)+cv(5)*latlat(k)+cv(6); + + end + + err = err + (efit-err_ll(k))^2; + +end \ No newline at end of file diff --git a/src/g_rect/g_pix2ll.m b/src/g_rect/g_pix2ll.m new file mode 100755 index 0000000000000000000000000000000000000000..cb4a93aa673ceba6735745883d64dec66d97817e --- /dev/null +++ b/src/g_rect/g_pix2ll.m @@ -0,0 +1,133 @@ +function [LON,LAT] = g_pix2ll(xp,yp,imgWidth,imgHeight,ic,jc,... + hfov,lambda,phi,theta,H,LON0,LAT0,field); +% G_PIX2LL Converts pixel to ground coordinates +% +% input: +% xp, yp: The image coordinate +% imgWidth: Number of horizontal pixel of the image +% imgHeight: Number of vertical pixel of the image +% ic, jc: The number of pixel off center for the principal point +% (generally both set to 0) +% hfov: Horizontal field of view +% lambda: Dip angle below horizontal (straight down = 90, horizontal = 0) +% phi: Tilt angle clockwise around the principal axis +% theta: View angle clockwise from North (e.g. East = 90) +% H: Camera altitude (m) above surface of interest. +% LON0, LAT0: Camera longitude and latitude position +% +% output: LAT,LON: Ground coordinates +% +% Authors: +% +% R. Pawlowicz 2002, University of British Columbia +% Reference: Pawlowicz, R. (2003) Quantitative visualization of +% geophysical flows using low-cost oblique digital +% time-lapse imaging, IEEE Journal of Oceanic Engineering +% 28 (4), 699-710. +% +% D. Bourgault 2012 - Naming convention slightly modified to match naming +% convention used in other part of the g_rect package. +% +% + +% Earth's radius (m) +Re = 6378135.0; + +% Transformation factors for local cartesian coordinate +meterPerDegLat = 1852*60.0; +meterPerDegLon = meterPerDegLat*cosd(LAT0); + +% Image aspect ratio. +aspectRatio = imgWidth/imgHeight; + +% Construct the image coordinate given the width and height of the image. +%xp = repmat([1:imgWidth]',1,imgHeight); +%yp = repmat([1:imgHeight],imgWidth,1); + +[n,m] = size(xp); + +% Image origin +x_c = imgWidth/2; +y_c = imgHeight/2; + +% Compute the vertical angle of view (vfov) given the horizontal angle +% of view (hfov) and the image aspect ratio. Then calculate the focal +% length (fx, fy). +% In principle, the horizontal and vertical focal lengths are identical. +% However these may slighty differ from cameras. The calculation done here +% provides identical focal length. + +vfov = 2*atand(tand(hfov/2)/aspectRatio); +fx = (imgWidth/2)/tand(hfov/2); +fy = (imgHeight/2)/tand(vfov/2); + + +% Subtract the principal point +x_p = xp - x_c + (jc); +y_p = yp - y_c + (ic); + +% Divide by the focal length +xd = x_p./fx; +yd = y_p./fy; + +x = xd; +y = yd; + +% The rotations are performed clockwise, first around the z-axis (rot), +% then around the already once rotated x-axis (dip) and finally around the +% twice rotated y-axis (tilt); + +% Tilt angle +R_phi = [ cosd(-phi), -sind(-phi), 0; + sind(-phi), cosd(-phi), 0; + 0, 0, 1]; + +% Dip angle +R_lambda = [ 1, 0, 0; + 0, cosd(-lambda), -sind(-lambda); + 0, sind(-lambda), cosd(-lambda)]; + +% View from North +R_theta = [ cosd(-theta), 0, -sind(-theta); + 0, 1, 0; + sind(-theta), 0, cosd(-theta)]; + +z = ones(size(x)); + +% Apply tilt and dip corrections + +p = R_lambda*R_phi*[(x(:))';(y(:))';(z(:))']; + +% Rotate towards true direction +M = R_theta*p; + +% Project forward onto ground plane (flat-earth distance) +alpha = H./M(2,:); +alpha(M(2,:) < 0) = NaN; % Blanks out vectors pointing above horizon + +% Need distance away and across field-of-view for auto-scaling +xx = alpha.*p(1,:); +zz = alpha.*p(3,:); + +x_w = reshape(alpha.*M(1,:),n,m); +z_w = reshape(alpha.*M(3,:),n,m); + +% Spherical earth corrections +Dfl2 = (x_w.^2 + z_w.^2); +Dhoriz2 = (2*H*Re); % Distance to spherical horizon + +fac = (4*Dfl2/Dhoriz2); +fac(fac >= 1.0) = NaN; % Points past horizon +s2f = 2*(1 - sqrt( 1 - fac )) ./ fac; + +x_w = x_w.*s2f; +z_w = z_w.*s2f; + +% Convert coordinates to lat/lon using locally cartesian assumption +if field + LON = x_w/meterPerDegLon + LON0; + LAT = z_w/meterPerDegLat + LAT0; +else + LON = x_w + LON0; + LAT = z_w + LAT0; +end diff --git a/src/g_rect/g_poly.m b/src/g_rect/g_poly.m new file mode 100755 index 0000000000000000000000000000000000000000..2c8e29d8d4b0da227210ad5374a72627a64c482c --- /dev/null +++ b/src/g_rect/g_poly.m @@ -0,0 +1,91 @@ +function [LONpc, LATpc, err_polyfit] = g_poly(LON,LAT,LON0,LAT0,i_gcp,j_gcp,lon_gcp,lat_gcp,p_order,field); + +ngcp=length(i_gcp); + +% LON0 and LAT0 are removed to facilitate the fiminsearch algorithm +for k = 1:ngcp + lonlon(k) = LON(i_gcp(k),j_gcp(k))-LON0; + latlat(k) = LAT(i_gcp(k),j_gcp(k))-LAT0; + err_lon(k) = lonlon(k)-(lon_gcp(k)-LON0); + err_lat(k) = latlat(k)-(lat_gcp(k)-LAT0); + + Delta_lon = LON(i_gcp(k)+1,j_gcp(k)) - LON(i_gcp(k),j_gcp(k)); + Delta_lat = LAT(i_gcp(k)+1,j_gcp(k)) - LAT(i_gcp(k),j_gcp(k)); + + if (abs(err_lon) - Delta_lon) < 0 + err_lon(k) = 0.0; + end + if (abs(err_lat) - Delta_lat) < 0 + err_lat(k) = 0.0; + end + +end +LON = LON-LON0; LAT = LAT-LAT0; + +options = optimset('MaxFunEvals',1000000,'MaxIter',1000000,'TolFun',1.d-12,'TolX',1.d-12); + +% The fminsearch is done twice for more accuracy. Otherwise, sometime it is +% not the true minimum that is found in the first pass. + +if p_order == 1 + + a(1:3) = 0.0; + b(1:3) = 0.0; + a = fminsearch('g_error_polyfit',a,options,lonlon,latlat,err_lon,p_order); + a = fminsearch('g_error_polyfit',a,options,lonlon,latlat,err_lon,p_order); + b = fminsearch('g_error_polyfit',b,options,lonlon,latlat,err_lat,p_order); + b = fminsearch('g_error_polyfit',b,options,lonlon,latlat,err_lat,p_order); + + LONpc = LON-(a(1)*LON+a(2)*LAT+a(3)); + LATpc = LAT-(b(1)*LON+b(2)*LAT+b(3)); + +elseif p_order == 2 + + % First pass - 1st order + a(1:3) = 0.0; + b(1:3) = 0.0; + a = fminsearch('g_error_polyfit',a,options,lonlon,latlat,err_lon,1); + a = fminsearch('g_error_polyfit',a,options,lonlon,latlat,err_lon,1); + b = fminsearch('g_error_polyfit',b,options,lonlon,latlat,err_lat,1); + b = fminsearch('g_error_polyfit',b,options,lonlon,latlat,err_lat,1); + + LONpc = LON-(a(1)*LON+a(2)*LAT+a(3)); + LATpc = LAT-(b(1)*LON+b(2)*LAT+b(3)); + + LON = LONpc + LON0; + LAT = LATpc + LAT0; + + % Second pass - 2nd order + for k = 1:ngcp + lonlon(k) = LON(i_gcp(k),j_gcp(k))-LON0; + latlat(k) = LAT(i_gcp(k),j_gcp(k))-LAT0; + err_lon(k) = lonlon(k)-(lon_gcp(k)-LON0); + err_lat(k) = latlat(k)-(lat_gcp(k)-LAT0); + end + LON = LON-LON0; LAT = LAT-LAT0; + + a(1:6) = 0.0; + b(1:6) = 0.0; + a = fminsearch('g_error_polyfit',a,options,lonlon,latlat,err_lon,2); + a = fminsearch('g_error_polyfit',a,options,lonlon,latlat,err_lon,2); + b = fminsearch('g_error_polyfit',b,options,lonlon,latlat,err_lat,2); + b = fminsearch('g_error_polyfit',b,options,lonlon,latlat,err_lat,2); + LONpc=LON-(a(1)*LON.^2+a(2)*LAT.^2+a(3)*LON.*LAT+a(4)*LON+a(5)*LAT+a(6)); + LATpc=LAT-(b(1)*LON.^2+b(2)*LAT.^2+b(3)*LON.*LAT+b(4)*LON+b(5)*LAT+b(6)); + +end + +% Add the LON0 and LAT0 +LONpc = LONpc + LON0; +LATpc = LATpc + LAT0; + +% Recalculate the error after correction. +err_polyfit=0; +for k = 1:ngcp + + DX = g_dist(LONpc(i_gcp(k),j_gcp(k)),LATpc(i_gcp(k),j_gcp(k)),lon_gcp(k),lat_gcp(k),field); + err_polyfit = err_polyfit + DX^2; + +end +err_polyfit = sqrt(err_polyfit/ngcp); + diff --git a/src/g_rect/g_rect.m b/src/g_rect/g_rect.m new file mode 100755 index 0000000000000000000000000000000000000000..ecb13b55d38ab03bc8caa185c6d9f801a3eacb8b --- /dev/null +++ b/src/g_rect/g_rect.m @@ -0,0 +1,388 @@ +function g_rect +%G_RECT - Main function for georectifying oblique images. +% +% Syntax: Simply type g_rect at the command line and follow the +% instructions. +% +% Inputs: +% The function G_RECT reads an input parameter file that contains all +% information required to perfrorm a georectification of a given image. +% The format of this parameter file is detailed on-line on the G_RECT +% Wiki page. +% +% Outputs: +% The function G_RECT creates an output file that contains the following +% variables: +% +% imgFname: The reference image for the georectification. +% +% firstImgFname: The first image of a sequence of images to which the +% georectification could be applied to. This is really +% just a comment. +% +% lastImgFname: The last image of a sequence of images to which the +% georectification could be applied to. This is really +% just a comment. +% +% LON: The main matrix that contain the longitude of each +% pixel of the referecne image (imgFname). Note that +% if the package is used to rectify lab images this +% matrix rather contains the distance in meters from +% a predefined x-origin. +% +% LAT: Same as LON but for the latitude. +% +% LON0: A scalar for the longitude of the camera or, in the +% case of a lab setup its cartesian coordinate in meters. +% +% LAT0: Same as LON0 but for the latitude. +% +% lon_gcp: A vector containing the longitude of each ground +% control points (GCP). For the lab case this is the +% cartesian coordinate in meters. +% +% lat_gcp: Same as lon_gcp for latitude. +% +% i_gcp: The horizontal index of the image ground control +% points. +% +% j_gcp: The vertical index of the image ground control +% points. +% +% hfov: The camera horizontal field of view [degree]. +% +% phi: Camera tilt angle [degree]. +% +% H: The camera altitude relative to the water [m]. +% +% theta: View angle clockwise from North [degree]. +% +% +% errGeoFit: The rms error of the georectified image after +% geometrical transformation [m]. +% +% errPolyFit: The rms error of the georectified image after +% geometrical transformation and the polynomial +% correction [m]. +% +% precision: Calculation can be done in single or double +% precisions as defined by the user in the parameter +% file. With today's computers this is now obsolete +% and calculations can always be done in double +% precision. +% +% +% Other m-files required: Works best with the m_map package for +% visualization. +% Subfunctions: all functions contained within the G_RECT folder. +% MAT-files required: none +% +% Author: Daniel Bourgault +% Institut des sciences de la mer de Rimouski +% email: daniel_bourgault@uqar.ca +% Website: http://demeter.uqar.ca/g_rect/ +% February 2013 +% + +% +% The minimization is repeated nMinimize times where each time a random +% combination of the initial guesses is chosen within the given +% uncertainties provided by the user. This is becasue the algorithm often +% converges toward a local minimum. The repetition is used to increase chances +% that the minimum found is a true minimum within the uncertainties provided. +nMinimize = 50; + + +%% Read the parameter file +% Count the number of header lines before the ground control points (GCPs) +% The GCPs start right after the variable gcpData is set to true. +display(' '); +display(' Welcome to g_rect: a package for georectifying oblique images on a flat ocean'); +display(' Authors: Daniel Bourgault and Rich Pawlowicz'); +display(' '); +inputFname = input(' Enter the filename for the input parameters: ','s'); +fid = fopen(inputFname); + +nHeaderLine = 0; +gcpData = false; + +% Read and execute each line of the parameter file until gcpData = true +% after which the GCP data appear and are read below with the importdata +% function. +while gcpData == false + eval(fgetl(fid)); + nHeaderLine = nHeaderLine + 1; +end +fclose(fid); + +%% Import the GCP data (4 column) at the end of the parameter file +gcp = importdata(inputFname,' ',nHeaderLine); +i_gcp = gcp.data(:,1); +j_gcp = gcp.data(:,2); +lon_gcp = gcp.data(:,3); +lat_gcp = gcp.data(:,4); +ngcp = length(i_gcp); + +% Get the image size +imgInfo = imfinfo(imgFname); +imgWidth = imgInfo.Width; +imgHeight = imgInfo.Height; + +if precision == 'single' + imgWidth = single(imgWidth); + imgHeight = single(imgHeight); +end + + +%% Display information +fprintf('\n') +fprintf(' INPUT PARAMETERS\n') +fprintf(' Image filename: (imgFname):........... %s\n',imgFname) +fprintf(' First image: (firstImgFname):......... %s\n',firstImgFname) +fprintf(' Last image: (lastImgFname):........... %s\n',lastImgFname) +fprintf(' Output filename: (outputFname):....... %s\n',outputFname); +fprintf(' Image width (imgWidth):............... %i\n',imgWidth) +fprintf(' Image width (imgHeight):.............. %i\n',imgHeight) +fprintf(' Camera longitude (LON0):.............. %f\n',LON0) +fprintf(' Camera latitude (LAT0):............... %f\n',LAT0) +fprintf(' Principal point offset (ic):.......... %f\n',ic) +fprintf(' Principal point offset (jc):.......... %f\n',jc) +fprintf(' Field of view (hfov):................. %f\n',hfov) +fprintf(' Dip angle (lambda):................... %f\n',lambda) +fprintf(' Tilt angle (phi):..................... %f\n',phi) +fprintf(' Camera altitude (H):.................. %f\n',H) +fprintf(' View angle from North (theta):........ %f\n',theta) +fprintf(' Uncertainty in hfov (dhfov):.......... %f\n',dhfov) +fprintf(' Uncertainty in dip angle (dlambda):... %f\n',dlambda) +fprintf(' Uncertainty in tilt angle (dphi):..... %f\n',dphi) +fprintf(' Uncertainty in altitude (dH):......... %f\n',dH) +fprintf(' Uncertainty in view angle (dtheta):... %f\n',dtheta) +fprintf(' Polynomial order (polyOrder):......... %i\n',polyOrder) +fprintf(' Number of GCPs (ngcp):................ %i\n',ngcp) +fprintf(' Precision (precision):................ %s\n',precision) +fprintf(' Field or lab (field=true; lab=false):. %i\n',field) +fprintf('\n') + +% Display the image with GCPs; +% image(imread(imgFname)); +imagesc(imread(imgFname)); +colormap(gray); +for i = 1:ngcp + text(i_gcp(i),j_gcp(i),num2str(i),'color','r','horizontalalignment','center'); +end +title('Ground Control Points','color','r'); + +print -dpng image1.png + +fprintf('\n') +ok = input('Is it ok to proceed with the rectification (y/n): ','s'); +if ok ~= 'y' + return + %break +end + +%% + +nUnknown = 0; +if dhfov > 0.0; nUnknown = nUnknown+1; end +if dlambda > 0.0; nUnknown = nUnknown+1; end +if dphi > 0.0; nUnknown = nUnknown+1; end +if dH > 0.0; nUnknown = nUnknown+1; end +if dtheta > 0.0; nUnknown = nUnknown+1; end + +if nUnknown > ngcp + fprintf('\n') + fprintf('WARNING: \n'); + fprintf(' The number of GCPs is < number of unknown parameters.\n'); + fprintf(' Program stopped.\n'); + %break + return +end + +% Check for consistencies between number of GCPs and order of the polynomial +% correction +ngcp = length(i_gcp); +if ngcp < 3*polyOrder + fprintf('\n') + fprintf('WARNING: \n'); + fprintf(' The number of GCPs is inconsistent with the order of the polynomial correction.\n'); + fprintf(' ngcp should be >= 3*polyOrder.\n'); + fprintf(' Polynomial correction will not be applied.\n'); + polyCorrection = false; +else + polyCorrection = true; +end +if polyOrder == 0 + polyCorrection = false; +end + +%% This is the main section for the minimization algorithm + +if nUnknown > 0 + + % Options for the fminsearch function. May be needed for some particular + % problems but in general the default values should work fine. + %options=optimset('MaxFunEvals',100000,'MaxIter',100000); + %options=optimset('MaxFunEvals',100000,'MaxIter',100000,'TolX',1.d-12,'TolFun',1.d-12); + options = []; + + + % Only feed the minimization algorithm with the GCPs. xp and yp are the + % image coordinate of these GCPs. + xp = i_gcp; + yp = j_gcp; + + % This is the call to the minimization + bestErrGeoFit = Inf; + + % Save inital guesses in new variables. + hfovGuess = hfov; + lambdaGuess = lambda; + phiGuess = phi; + HGuess = H; + thetaGuess = theta; + + for iMinimize = 1:nMinimize + + % First guesses for the minimization + if iMinimize == 1 + hfov0 = hfov; + lambda0 = lambda; + phi0 = phi; + H0 = H; + theta0 = theta; + else + % Select randomly new initial guesses within the specified + % uncertainties. + hfov0 = (hfovGuess - dhfov) + 2*dhfov*rand(1); + lambda0 = (lambdaGuess - dlambda) + 2*dlambda*rand(1); + phi0 = (phiGuess - dphi) + 2*dphi*rand(1); + H0 = (HGuess - dH) + 2*dH*rand(1); + theta0 = (thetaGuess - dtheta) + 2*dtheta*rand(1); + end + + % Cretae vector cv0 for the initial guesses. + i = 0; + if dhfov > 0.0 + i = i+1; + cv0(i) = hfov0; + theOrder(i) = 1; + end + if dlambda > 0.0 + i = i + 1; + cv0(i) = lambda0; + theOrder(i) = 2; + end + if dphi > 0.0 + i = i + 1; + cv0(i) = phi0; + theOrder(i) = 3; + end + if dH > 0.0 + i = i + 1; + cv0(i) = H0; + theOrder(i) = 4; + end + if dtheta > 0.0 + i = i + 1; + cv0(i) = theta0; + theOrder(i) = 5; + end + + [cv, errGeoFit] = fminsearch('g_error_geofit',cv0,options, ... + imgWidth,imgHeight,xp,yp,ic,jc,... + hfov,lambda,phi,H,theta,... + hfov0,lambda0,phi0,H0,theta0,... + hfovGuess,lambdaGuess,phiGuess,HGuess,thetaGuess,... + dhfov,dlambda,dphi,dH,dtheta,... + LON0,LAT0,... + i_gcp,j_gcp,lon_gcp,lat_gcp,... + theOrder,field); + + + if errGeoFit < bestErrGeoFit + bestErrGeoFit = errGeoFit; + cvBest = cv; + end + + fprintf('\n') + fprintf(' Iteration # (iMinimize): %i\n',iMinimize); + fprintf(' Max. number of iteration (nMinimize): %i\n',nMinimize); + fprintf(' RSM error (m) for this iteration (errGeoFit): %f\n',errGeoFit); + fprintf(' Best RSM error (m) so far (bestErrGeoFit): %f\n',bestErrGeoFit); + + end + + for i = 1:length(theOrder) + if theOrder(i) == 1; hfov = cvBest(i); end + if theOrder(i) == 2; lambda = cvBest(i); end + if theOrder(i) == 3; phi = cvBest(i); end + if theOrder(i) == 4; H = cvBest(i); end + if theOrder(i) == 5; theta = cvBest(i); end + end + + fprintf('\n') + fprintf('PARAMETERS AFTER GEOMETRICAL RECTIFICATION \n') + fprintf(' Field of view (hfov): %f\n',hfov) + fprintf(' Dip angle (lambda): %f\n',lambda) + fprintf(' Tilt angle (phi): %f\n',phi) + fprintf(' Camera altitude (H): %f\n',H) + fprintf(' View angle from North (theta): %f\n',theta) + fprintf('\n') + + if length(theOrder) > 1 + fprintf('The rms error after geometrical correction (m): %f\n',bestErrGeoFit); + end + +end + +%% + +% Now construct the matrices LON and LAT for the entire image using the +% camera parameters found by minimization just above. + +% Camera coordinate of all pixels +xp = repmat([1:imgWidth]',1,imgHeight); +yp = repmat([1:imgHeight],imgWidth,1); + +% Transform camera coordinate to ground coordinate. +[LON LAT] = g_pix2ll(xp,yp,imgWidth,imgHeight,ic,jc,... + hfov,lambda,phi,theta,H,LON0,LAT0,field); + + +%% Apply polynomial correction if requested. +if polyCorrection == true + [LON LAT errPolyFit] = g_poly(LON,LAT,LON0,LAT0,i_gcp,j_gcp,lon_gcp,lat_gcp,polyOrder,field); + fprintf('The rms error after polynomial stretching (m): %f\n',errPolyFit) +else + errPolyFit = NaN; +end +%% + +fprintf('\n') +fprintf('Saving rectification file in: %s\n',outputFname); + +errGeoFit = 0.0; + +save(outputFname,'imgFname','firstImgFname','lastImgFname',... + 'LON','LAT',... + 'LON0','LAT0',... + 'lon_gcp','lat_gcp',... + 'i_gcp','j_gcp',... + 'hfov','lambda','phi','H','theta',... + 'errGeoFit','errPolyFit',... + 'precision'); + +clear LON LAT + +fprintf('\n') +fprintf('Making figure\n'); + +figure; +if field + g_viz_field(imgFname,outputFname); +else + g_viz_lab(imgFname,outputFname); +end + +print -dpng image2.png \ No newline at end of file diff --git a/src/g_rect/g_res.m b/src/g_rect/g_res.m new file mode 100755 index 0000000000000000000000000000000000000000..d41bc66d648bd14ccbcb0611a5a1244d68536ccd --- /dev/null +++ b/src/g_rect/g_res.m @@ -0,0 +1,28 @@ +%function Delta = g_res(LON,LAT,i,j,field); +function [Delta DeltaX DeltaY] = g_res(LON,LAT,i,j,field); + +[m n] = size(LON); + +if i == 1 + Delta1 = g_dist(LON(i+1,j),LAT(i+1,j),LON(i,j),LAT(i,j),field); +elseif i == m + Delta1 = g_dist(LON(i,j),LAT(i,j),LON(i-1,j),LAT(i-1,j),field); +else + Delta1 = g_dist(LON(i+1,j),LAT(i+1,j),LON(i-1,j),LAT(i-1,j),field); + Delta1 = Delta1/2; +end + +if j == 1 + Delta2 = g_dist(LON(i,j+1),LAT(i,j+1),LON(i,j),LAT(i,j),field); +elseif j == n + Delta2 = g_dist(LON(i,j),LAT(i,j),LON(i,j-1),LAT(i,j-1),field); +else + Delta2 = g_dist(LON(i,j+1),LAT(i,j+1),LON(i,j-1),LAT(i,j-1),field); + Delta2 = Delta2/2; +end + +Delta = sqrt(Delta1^2 + Delta2^2); +DeltaX = sqrt(Delta1^2); +DeltaY = sqrt(Delta2^2); + +end \ No newline at end of file diff --git a/src/g_rect/g_viz_field.m b/src/g_rect/g_viz_field.m new file mode 100644 index 0000000000000000000000000000000000000000..45b7a25f09eb331d5983bcff4e4b903f19428579 --- /dev/null +++ b/src/g_rect/g_viz_field.m @@ -0,0 +1,69 @@ +function g_viz_field(imgFname,outputFname); + +% Set some plotting parameters +ms = 10; % Marker Size +fs = 10; % Font size +lw = 2; % Line width + +load(outputFname); + +[m n p] = size(LON); + +lon_min = min(lon_gcp); +lon_max = max(lon_gcp); +lat_min = min(lat_gcp); +lat_max = max(lat_gcp); +lon_min = min(lon_min,LON0); +lon_max = max(lon_max,LON0); +lat_min = min(lat_min,LAT0); +lat_max = max(lat_max,LAT0); + +fac = 0.1; +lon_min= lon_min - fac*abs(lon_max-lon_min); +lon_max= lon_max + fac*abs(lon_max-lon_min); +lat_min= lat_min - fac*abs(lat_max-lat_min); +lat_max= lat_max + fac*abs(lat_max-lat_min); + +rgb0 = double(imread(imgFname))/255; + +[mm nn pp] = size(rgb0); +if pp == 3 + int = (rgb0(:,:,1)+rgb0(:,:,2)+rgb0(:,:,3))/3; +else + int = rgb0; +end +clear rgb0; +int = int'; + +int = int - nanmean(nanmean(int)); + +land_color = [241 235 144]/255; + +m_proj('mercator','longitudes',[lon_min lon_max],'latitudes',[lat_min lat_max]); +hold on; + +[X,Y] = m_ll2xy(LON,LAT); + +colormap(gray); +h = pcolor(X,Y,int); +shading('interp'); + + +% Uncomment one of these lines if you want the coastline to be pltoted. +%m_gshhs_f('patch',land_color) +m_gshhs_h('patch',land_color) + +m_plot(LON0,LAT0,'kx','markersize',ms,'linewidth',lw); % Camera location + +%% Plot GCPs and ICPs. +for n=1:length(i_gcp) + m_plot(lon_gcp(n),lat_gcp(n),'bo','markersize',ms,'linewidth',lw); + m_plot(LON(i_gcp(n),j_gcp(n)),LAT(i_gcp(n),j_gcp(n)),'rx','MarkerSize',ms,'linewidth',lw); +end + +%title([datestr(mtime,31),' UTC']); +%title([time,' UTC']); + +clear LON LAT X Y + +m_grid('box','fancy','fontsize',fs); \ No newline at end of file diff --git a/src/g_rect/g_viz_lab.m b/src/g_rect/g_viz_lab.m new file mode 100644 index 0000000000000000000000000000000000000000..21309e4467c71e4c2d430e2446c3e7877fdf1bfe --- /dev/null +++ b/src/g_rect/g_viz_lab.m @@ -0,0 +1,52 @@ +function g_viz_lab(imgFname,outputFname); + +% Set some plotting parameters +ms = 10; % Marker Size +fs = 10; % Font size +lw = 2; % Line width + +load(outputFname); + +lon_min = min(lon_gcp); +lon_max = max(lon_gcp); +lat_min = min(lat_gcp); +lat_max = max(lat_gcp); +lon_min = min(lon_min,LON0); +lon_max = max(lon_max,LON0); +lat_min = min(lat_min,LAT0); +lat_max = max(lat_max,LAT0); + +fac = 0.1; +lon_min= lon_min - fac*abs(lon_max-lon_min); +lon_max= lon_max + fac*abs(lon_max-lon_min); +lat_min= lat_min - fac*abs(lat_max-lat_min); +lat_max= lat_max + fac*abs(lat_max-lat_min); + +rgb0 = double(imread(imgFname))/255; + +[mm nn pp] = size(rgb0); +if pp == 3 + int = (rgb0(:,:,1)+rgb0(:,:,2)+rgb0(:,:,3))/3; +else + int = rgb0; +end +clear rgb0; +int = int'; + +hold on; + +X = LON; +Y = LAT; + +colormap(gray); +h = pcolor(X,Y,int); +shading('flat'); + + +plot(LON0,LAT0,'kx','markersize',ms,'linewidth',lw); % Camera location + +%% Plot GCPs and ICPs. +for n=1:length(i_gcp) + plot(lon_gcp(n),lat_gcp(n),'bo','markersize',ms,'linewidth',lw); + plot(LON(i_gcp(n),j_gcp(n)),LAT(i_gcp(n),j_gcp(n)),'rx','MarkerSize',ms,'linewidth',lw); +end diff --git a/src/g_stabilize/Farid_Woodward_2007.pdf b/src/g_stabilize/Farid_Woodward_2007.pdf new file mode 100644 index 0000000000000000000000000000000000000000..819ba3f786ada56cd0c35d4676fef374934f115f Binary files /dev/null and b/src/g_stabilize/Farid_Woodward_2007.pdf differ diff --git a/src/g_stabilize/g_accumulatewarp.m b/src/g_stabilize/g_accumulatewarp.m new file mode 100644 index 0000000000000000000000000000000000000000..03c81b3ae0639a7ae5db35c6a6a4da641835bef2 --- /dev/null +++ b/src/g_stabilize/g_accumulatewarp.m @@ -0,0 +1,3 @@ +function[ A2, T2 ] = g_accumulatewarp( Acum, Tcum, A, T ) +A2 = A * Acum; +T2 = A*Tcum + T; \ No newline at end of file diff --git a/src/g_stabilize/g_computemotion.m b/src/g_stabilize/g_computemotion.m new file mode 100644 index 0000000000000000000000000000000000000000..6fc9e0cc7435051c0fa32fa9ecac76d2168bcfaa --- /dev/null +++ b/src/g_stabilize/g_computemotion.m @@ -0,0 +1,43 @@ +function[ A, T ] = g_computemotion( fx, fy, ft, roi ) +[ydim,xdim] = size(fx); +[x,y] = meshgrid( [1:xdim]-xdim/2, [1:ydim]-ydim/2 ); + +%%% TRIM EDGES +fx = fx( 3:end-2, 3:end-2 ); +fy = fy( 3:end-2, 3:end-2 ); +ft = ft( 3:end-2, 3:end-2 ); +roi = roi( 3:end-2, 3:end-2 ); +x = x( 3:end-2, 3:end-2 ); +y = y( 3:end-2, 3:end-2 ); + +ind = find( roi > 0 ); +x = x(ind); y = y(ind); +fx = fx(ind); fy = fy(ind); ft = ft(ind); + +xfx = x.*fx; xfy = x.*fy; yfx = y.*fx; yfy = y.*fy; +M(1,1) = sum( xfx .* xfx ); M(1,2) = sum( xfx .* yfx ); M(1,3) = sum( xfx .* xfy ); +M(1,4) = sum( xfx .* yfy ); M(1,5) = sum( xfx .* fx ); M(1,6) = sum( xfx .* fy ); +M(2,1) = M(1,2); M(2,2) = sum( yfx .* yfx ); M(2,3) = sum( yfx .* xfy ); +M(2,4) = sum( yfx .* yfy ); M(2,5) = sum( yfx .* fx ); M(2,6) = sum( yfx .* fy ); +M(3,1) = M(1,3); M(3,2) = M(2,3); M(3,3) = sum( xfy .* xfy ); +M(3,4) = sum( xfy .* yfy ); M(3,5) = sum( xfy .* fx ); M(3,6) = sum( xfy .* fy ); +M(4,1) = M(1,4); M(4,2) = M(2,4); M(4,3) = M(3,4); +M(4,4) = sum( yfy .* yfy ); M(4,5) = sum( yfy .* fx ); M(4,6) = sum( yfy .* fy ); +M(5,1) = M(1,5); M(5,2) = M(2,5); M(5,3) = M(3,5); +M(5,4) = M(4,5); M(5,5) = sum( fx .* fx ); M(5,6) = sum( fx .* fy ); +M(6,1) = M(1,6); M(6,2) = M(2,6); M(6,3) = M(3,6); +M(6,4) = M(4,6); M(6,5) = M(5,6); M(6,6) = sum( fy .* fy ); + +k = ft + xfx + yfy; + +b(1) = sum( k .* xfx ); b(2) = sum( k .* yfx ); +b(3) = sum( k .* xfy ); b(4) = sum( k .* yfy ); +b(5) = sum( k .* fx ); b(6) = sum( k .* fy ); + +%v = inv(M) * b'; +v = M\b'; + +%A = [1 0 ; 0 1]; + +A = [v(1) v(2) ; v(3) v(4)]; +T = [v(5) ; v(6)]; \ No newline at end of file diff --git a/src/g_stabilize/g_down.m b/src/g_stabilize/g_down.m new file mode 100644 index 0000000000000000000000000000000000000000..49eb42f23f417796bf42d2ed2d75182c5fa08a4a --- /dev/null +++ b/src/g_stabilize/g_down.m @@ -0,0 +1,6 @@ +function[ f ] = g_down( f, L ); +blur = [1 2 1]/4; +for k = 1 : L +f = conv2( conv2( f, blur, 'same' ), blur', 'same' ); +f = f(1:2:end,1:2:end); +end \ No newline at end of file diff --git a/src/g_stabilize/g_opticalflow.m b/src/g_stabilize/g_opticalflow.m new file mode 100644 index 0000000000000000000000000000000000000000..99a4caebc12d125a5a6c63ff5ebdd2d9a567dc16 --- /dev/null +++ b/src/g_stabilize/g_opticalflow.m @@ -0,0 +1,21 @@ +function[ Acum, Tcum ] = g_opticalflow( f1, f2, roi, L ) +f2orig = f2; +Acum = [1 0 ; 0 1]; +Tcum = [0 ; 0]; + +for k = L : -1 : 0 +%for k = L : -1 : L + + %%% DOWN-SAMPLE + f1d = g_down( f1, k ); + f2d = g_down( f2, k ); + ROI = g_down( roi, k ); + %%% COMPUTE MOTION + [Fx,Fy,Ft] = g_spacetimederiv( f1d, f2d ); + [A,T] = g_computemotion( Fx, Fy, Ft, ROI ); + T = (2^k) * T; + [Acum,Tcum] = g_accumulatewarp( Acum, Tcum, A, T ); + %%% WARP ACCORDING TO ESTIMATED MOTION + f2 = g_warp( f2orig, Acum, Tcum ); + +end \ No newline at end of file diff --git a/src/g_stabilize/g_roi.m b/src/g_stabilize/g_roi.m new file mode 100644 index 0000000000000000000000000000000000000000..57c844b233b852a6ff25129f5d060c19e18ff0f0 --- /dev/null +++ b/src/g_stabilize/g_roi.m @@ -0,0 +1,61 @@ +function g_roi(imgFname); + +% G_ROI is a function to determine regions of interest in an image. +% +% This function allows the user to define a series of polygons +% on an image in order to define regions of interest (roi) for +% image stabilization. The result is a 2D matrix roi with 1s and 0s. +% Just follow ths instructions. +% +% Input: imgFname: the image filename +% Output: The 2D matrix roi written to filename roi.mat +% +% Author: Daniel Bourgault - 2011 +% +% + +im = imread(imgFname); +imagesc(im); +colormap(gray); +hold on; + +[m n p] = size(im); + +roi(1:m,1:n) = 0.0; + +I = repmat([1:m]',1,n); +J = repmat([1:n],m,1); + +answer='y'; +while answer=='y' + + display(' '); + input(' Position the image as you wish before defining a polygon. Press ENTER when ready.'); + + display(' '); + display(' Make a polygon with the mouse. Press ENTER when done with this polygon.') + display(' You will have the option to make more polygons when done with this one.') + + [px py] = ginput; + + px(end+1) = px(1); + py(end+1) = py(1); + + plot(px,py,'r'); + + roiTmp = inpolygon(I,J,py,px); + + display(' '); + answer = input(' Enter another polygon (y/n)? ','s'); + + roi = roi + roiTmp; + +end + +ij = find(roi > 1); +roi(ij) = 1; + +figure(2); +imagesc(roi); + +save roi.mat roi imgFname \ No newline at end of file diff --git a/src/g_stabilize/g_spacetimederiv.m b/src/g_stabilize/g_spacetimederiv.m new file mode 100644 index 0000000000000000000000000000000000000000..31025fb6e1f809fae14c515970049447fb99a2aa --- /dev/null +++ b/src/g_stabilize/g_spacetimederiv.m @@ -0,0 +1,10 @@ +function[ fx, fy, ft ] = g_spacetimederiv( f1, f2 ) +%%% DERIVATIVE FILTERS +pre = [0.5 0.5]; +deriv = [0.5 -0.5]; +%%% SPACE/TIME DERIVATIVES +fpt = pre(1)*f1 + pre(2)*f2; % pre-filter in time +fdt = deriv(1)*f1 + deriv(2)*f2; % differentiate in time +fx = conv2( conv2( fpt, pre', 'same' ), deriv, 'same' ); +fy = conv2( conv2( fpt, pre, 'same' ), deriv', 'same' ); +ft = conv2( conv2( fdt, pre', 'same' ), pre, 'same' ); diff --git a/src/g_stabilize/g_stabilize.m b/src/g_stabilize/g_stabilize.m new file mode 100644 index 0000000000000000000000000000000000000000..a6427cbce77b1352ec704d51b66611ca721d0a43 --- /dev/null +++ b/src/g_stabilize/g_stabilize.m @@ -0,0 +1,113 @@ +function g_stabilize(root_img_name,ext,id_img_ref,id_img_first,id_img_last,precision); +% Function that calls the stabilization algorithm. +% +% Input: +% root_img_fname: The root name of the images before the id number. +% ext: The image extension (.jpg .png etc) +% id_img_ref: The id number of the reference image +% id_img_first: The id number of the first image of the sequence to +% be stabilized +% id_img_last: The id number of the last image of the sequence to +% be stabilized +% precision: The number of digit for the file name +% (e.g.: IMG_0400.JPG would be 4). +% +% Output: +% All stabilized images are rewritten as imaghe files. +% + +fname_suffix = '_stable'; + +% Region of interest +display(' Load the roi.mat file containing the region of interest (roi)'); +load roi.mat; + + +ndigit = ['%',num2str(precision),'.',num2str(precision),'i']; + +% Read the reference image +img_ref = [root_img_name,num2str(id_img_ref,ndigit),ext]; +im2 = imread(img_ref); +im2 = im2(:,:,1); + + +% Pyramid level for the stabilization. +% See documentation in the g_stabilize folder. +% L = 4 worked well on initial application but may require adjustments. +L = 4; + +% Number of iterative improvement. Probably ok just 1 iteration. +niter = 1; + +% Parameters for equalization. See help clahs for details. +nry = 4; +nrx = 4; +im2 = clahs(im2,nry,nrx); +im2 = double(im2); + +% Remove mean and divide by the standard deviation. +inorm = find(roi > 0); +im2_norm = (im2 - nanmean(im2(inorm)))./nanstd(im2(inorm)); + +% Take the norm of the gradient of the image. This helps the +% stabilization algorithm +[im2x im2y] = gradient(im2_norm); +grad_im2 = sqrt(im2x.^2 + im2y.^2); + +% This is the referecne frame for the stabilization algorithm. +frames(2).im = grad_im2; + + +figure(1); +imagesc(im2); +colormap(gray); +title('Reference image normalized intensity'); + +figure(2); +imagesc(grad_im2); +colormap(gray); +title('Gradient of the referemce image'); + + +figure(3); +imagesc(roi); +title('roi'); + +answer = input('Happy with roi (y/n)? ','s'); +if answer == 'n' + return +end + + +for i = id_img_first:id_img_last + + i + + %img_to_stabilize = [root_img_name,num2str(i+1),ext]; + img_to_stabilize = [root_img_name,num2str(i,ndigit),ext]; + im1 = imread(img_to_stabilize); + im1 = im1(:,:,1); + + im1 = clahs(im1,nry,nrx); + + im1 = double(im1); + im1_norm = (im1 - nanmean(im1(inorm)))./nanstd(im1(inorm)); + [im1x im1y] = gradient(im1_norm); + grad_im1 = sqrt(im1x.^2 + im1y.^2); + + frames(1).im = grad_im1; + + for iter = 1:niter + [motion,stable] = g_videostabilize(frames,roi,L); + frames(1).im = stable(1).im; + %motion.A + %motion.T + end + im_stable = g_warp(im1,motion.A,motion.T); + im_stable = uint8(im_stable); + %frames(2).im = stable(1).im; + + %imwrite(im_stable,[img_to_stabilize(1:end-4),fname_suffix,ext],'Quality',100); + imwrite(im_stable,[img_to_stabilize(1:end-4),fname_suffix,'.jpg'],'Quality',100); + +end diff --git a/src/g_stabilize/g_videostabilize.m b/src/g_stabilize/g_videostabilize.m new file mode 100644 index 0000000000000000000000000000000000000000..7c43be0f716c948c058915f8adb419e9dc2314f6 --- /dev/null +++ b/src/g_stabilize/g_videostabilize.m @@ -0,0 +1,23 @@ +%%% STABILIZE VIDEO +function[ motion, stable ] = g_videostabilize( frames, roi, L ) +N = length( frames ); +roiorig = roi; +%%% ESTIMATE PAIRWISE MOTION +Acum = [1 0 ; 0 1]; +Tcum = [0 ; 0]; +stable(1).roi = roiorig; +for k = 1 : N-1 + [A,T] = g_opticalflow( frames(k+1).im, frames(k).im, roi, L ); + motion(k).A = A; + motion(k).T = T; + [Acum,Tcum] = g_accumulatewarp( Acum, Tcum, A, T ); + roi = g_warp( roiorig, Acum, Tcum ); +end +%%% STABILIZE TO LAST FRAME +stable(N).im = frames(N).im; +Acum = [1 0 ; 0 1]; +Tcum = [0 ; 0]; +for k = N-1 : -1 : 1 + [Acum,Tcum] = g_accumulatewarp( Acum, Tcum, motion(k).A, motion(k).T ); + stable(k).im = g_warp( frames(k).im, Acum, Tcum ); +end diff --git a/src/g_stabilize/g_warp.m b/src/g_stabilize/g_warp.m new file mode 100644 index 0000000000000000000000000000000000000000..bd5f380d9de191a99adf8f48adf31d589b100ae4 --- /dev/null +++ b/src/g_stabilize/g_warp.m @@ -0,0 +1,16 @@ +function[ f2 ] = g_warp( f, A, T ) + +Method = 'cubic'; + +[ydim,xdim] = size( f ); +[xramp,yramp] = meshgrid( [1:xdim]-xdim/2, [1:ydim]-ydim/2 ); + +P = [xramp(:)' ; yramp(:)']; +P = A * P; + +xramp2 = reshape( P(1,:), ydim, xdim ) + T(1); +yramp2 = reshape( P(2,:), ydim, xdim ) + T(2); +%f2 = interp2( xramp, yramp, f, xramp2, yramp2, 'bicubic' ); % warp +f2 = interp2( xramp, yramp, f, xramp2, yramp2, Method ); % warp +ind = find( isnan(f2) ); +f2(ind) = 0; \ No newline at end of file diff --git a/src/utilities/uwit/blending/_afs22E7 b/src/utilities/uwit/blending/_afs22E7 new file mode 100644 index 0000000000000000000000000000000000000000..e69de29bb2d1d6434b8b29ae775ad8c2e48c5391 diff --git a/src/utilities/uwit/blending/blend.m b/src/utilities/uwit/blending/blend.m new file mode 100644 index 0000000000000000000000000000000000000000..5ce2006f8fea94b0c486f2d76e68fd69ee51c5b0 --- /dev/null +++ b/src/utilities/uwit/blending/blend.m @@ -0,0 +1,72 @@ +function [S] = blend(A,B,R) +%BLEND blends two images across their transistion zone +% S = BLEND(A,B,R) blends the two images A and B +% according to the mask R. R is an array of the same +% size as A and B and contains 1's at indices corresponding +% to regions of A to be blended into B. +% +% example: +% +% A = imread('rice.tif'); +% B = uint8(double(A)-40); % darken image +% C = A; +% C(:,end/2:end) = B(:,end/2:end); % create composite image +% figure; imshow(C); title('Unblended'); +% R = zeros(size(A)); +% R(:,1:end/2-1) = 1; +% D = blend(A,B,R); +% figure; imshow(D); title('Blended'); + +type = class(A); +A = double(A); +B = double(B); +R = double(R); +N = numlevels(A); +%================================== +% construct the laplacian pyramid +% for each image +%================================== +LA = pyramid(A,N); +LB = pyramid(B,N); + +%================================== +% construct the gaussian pyramid +% for the mask R +%================================== +GR = cell(N,1); +GR{1} = R; +for ii = 2:N + GR{ii} = reduce(GR{ii-1}); +end + +%================================== +% construct the laplacian pyramid +% for the splined image S +%================================== +LS = cell(N,1); +for ii = 1:N + LS{ii} = GR{ii}.*LA{ii} + (1-GR{ii}).*LB{ii}; +end + +%================================== +% reconstruct the splined image S +% from the laplacian pyramid LS +%================================== +S = deflate(LS); + +%================================== +% convert S to the same class type +% as the input images +%================================== +cmd = sprintf('S = %s(S);',type); +eval(cmd); + + +function [N] = numlevels(A); +% returns the ideal number of levels used in +% generating the laplacian pyramid +% for images of size (Mr*2^N + 1) by (Mc*2^N + 1) +% where Mr,Mc are integers, then the pyramid should +% contains N+1 levels. +[rowA colA] = size(A); +N = min(floor(log2(rowA)),floor(log2(colA))); \ No newline at end of file diff --git a/src/utilities/uwit/blending/blend_demo.m b/src/utilities/uwit/blending/blend_demo.m new file mode 100644 index 0000000000000000000000000000000000000000..611d0d571fc2dfd366b30201987a10085ef37bac --- /dev/null +++ b/src/utilities/uwit/blending/blend_demo.m @@ -0,0 +1,46 @@ +%BLEND_DEMO script + +load blend_demo +BASE = double(BASE); +INPUT = double(INPUT); + +path(path,'../blending'); + +[i j] = find(BASE>-inf); + +image_pair = [input_points base_points]; +im_xy = image_pair(:,1:2); +im_xy(:,3) = 1; +im_uv = image_pair(:,3:4); +im_uv(:,3) = 1; +xmatrix = im_xy \ im_uv; + + +JI = [j i ones(length(i),1)]; +VU = JI*xmatrix; +v = VU(:,1); +u = VU(:,2); + +tform = cp2tform(input_points,base_points,'affine'); + +[WARP X Y] = imtransform(INPUT,tform); +[rowB colB] = size(BASE); +[rowW colW] = size(WARP); +MOSAIC = zeros(rowB+127,colB+25); +MOSAIC(126:126+rowW-1,1:colW) = WARP; +MOSAIC(1:rowB,26:end) = BASE; +figure(1); +imagesc(MOSAIC);colormap gray;grid on;truesize; +title('MOSAIC UNBLENDED'); + +TOP = zeros(rowB+127,colB+25); +BOTTOM = TOP; +MASK = TOP; +MASK(150:end,1:570) = 1; +TOP(1:rowB,26:end) = BASE; +BOTTOM(126:126+rowW-1,1:colW) = WARP; + +MOSAIC_BLENDED = blend(BOTTOM,TOP,MASK); +figure(2); +imagesc(MOSAIC_BLENDED);colormap gray;grid on;truesize; +title('MOSAIC BLENDED'); \ No newline at end of file diff --git a/src/utilities/uwit/blending/blend_demo.mat b/src/utilities/uwit/blending/blend_demo.mat new file mode 100644 index 0000000000000000000000000000000000000000..13d1fb47a6ab04f847c44294b3a24fbb1c5ff5f7 Binary files /dev/null and b/src/utilities/uwit/blending/blend_demo.mat differ diff --git a/src/utilities/uwit/blending/deflate.m b/src/utilities/uwit/blending/deflate.m new file mode 100644 index 0000000000000000000000000000000000000000..4a805aea2c4c14f79b4805a7d1a8843307f8ff1b --- /dev/null +++ b/src/utilities/uwit/blending/deflate.m @@ -0,0 +1,10 @@ +function [I] = deflate(L); +% DEFLATE produces an image array from a laplacian pyramid +% I = DEFLATE(L) produces a single output array I, which +% is the result of expanding and adding the length(L) levels +% of the laplacian pyramid L. + +for ii = length(L):-1:2 + L{ii-1} = L{ii-1} + expand(L{ii},size(L{ii-1})); +end +I = L{1}; \ No newline at end of file diff --git a/src/utilities/uwit/blending/expand.m b/src/utilities/uwit/blending/expand.m new file mode 100644 index 0000000000000000000000000000000000000000..8a19ce24c3b2eef805ff4e7bddd0ef71ee7ac124 --- /dev/null +++ b/src/utilities/uwit/blending/expand.m @@ -0,0 +1,37 @@ +function [Gj] = expand(Gi,dim) + +rr = dim(1); +cc = dim(2); +[NrRows NrCols] = size(Gi); + +%================================== +% E is the extrapolated image which +% imposes boundary conditions +%================================== +E = zeros(NrRows+4,NrCols+4); +E(3:end-2,3:end-2) = Gi; +% top and bottom row +E(2,:) = 2*E(3,:) - E(4,:); +E(1,:) = 2*E(3,:) - E(5,:); +E(end-1,:) = 2*E(end-2,:) - E(end-3,:); +E(end,:) = 2*E(end-2,:) - E(end-4,:); +% left and right col +E(:,2) = 2*E(:,3) - E(:,4); +E(:,1) = 2*E(:,3) - E(:,5); +E(:,end-1) = 2*E(:,end-2) - E(:,end-3); +E(:,end) = 2*E(:,end-2) - E(:,end-4); + +[NRe NCe] = size(E); + +%==================================== +% upsample and interpolate +% intermediate points +%==================================== + +Ginterp = imresize(E,[(rr+8) (cc+8)],'bilinear'); + +%====================================== +% cut out central portion corresponding +% to upsampled original image +%====================================== +Gj = Ginterp(5:5+rr-1,5:5+cc-1); \ No newline at end of file diff --git a/src/utilities/uwit/blending/pyramid.m b/src/utilities/uwit/blending/pyramid.m new file mode 100644 index 0000000000000000000000000000000000000000..772ba0f400fb8f04538a28fd8a1e6c7982b0278d --- /dev/null +++ b/src/utilities/uwit/blending/pyramid.m @@ -0,0 +1,21 @@ +function [L] = pyramid(G0,N,varargin); +%PYRAMID generates the laplacian image pyramid +% L = PYRAMID(G0,N) takes as arguments the original +% image G0 and the number of levels N. It then computes +% the corresponding N level pyramid L. L is a N-by-1 +% cell array with each cell containing the Kth pyramid +% level. + +if nargin == 2 + L = cell(N,1); +else + L = varargin{1}; +end + +if N == 1 + L{end} = G0; +else + G1 = reduce(G0); + L{length(L)-N+1} = G0 - expand(G1,size(G0)); + L = pyramid(G1,N-1,L); +end \ No newline at end of file diff --git a/src/utilities/uwit/blending/reduce.m b/src/utilities/uwit/blending/reduce.m new file mode 100644 index 0000000000000000000000000000000000000000..52108ba880a075fba0a3d40eadc78647cfe67d1f --- /dev/null +++ b/src/utilities/uwit/blending/reduce.m @@ -0,0 +1,51 @@ +function [Gj] = reduce(Gi) +a = 0.4; +w = [(1/4-a/2) 1/4 a 1/4 (1/4-a/2)]; + +[NrRows NrCols] = size(Gi); + +%================================== +% E is the extrapolated image which +% imposes boundary conditions +%================================== +E = zeros(NrRows+4,NrCols+4); +E(3:end-2,3:end-2) = Gi; +% top and bottom row +E(2,:) = 2*E(3,:) - E(4,:); +E(1,:) = 2*E(3,:) - E(5,:); +E(end-1,:) = 2*E(end-2,:) - E(end-3,:); +E(end,:) = 2*E(end-2,:) - E(end-4,:); +% left and right col +E(:,2) = 2*E(:,3) - E(:,4); +E(:,1) = 2*E(:,3) - E(:,5); +E(:,end-1) = 2*E(:,end-2) - E(:,end-3); +E(:,end) = 2*E(:,end-2) - E(:,end-4); + +%================================== +% F = E*W +% convolve E with the kernel W +% (i.e. W=w*w') seperably +%================================== +[NRe NCe] = size(E); +Nw = length(w); +if 1 + F = zeros(NRe+Nw-1,NCe+Nw-1); + for ii = 1:NrRows + F(ii+2,:) = conv(w,E(ii+2,:)); + end + for jj = 1:NrCols + F(:,jj+2) = conv(w,F(3:end-2,jj+2)); + end +end + +%=================================== +% remove extra elements resultant +% from convolution +%=================================== +K = F(Nw:end-Nw+1,Nw:end-Nw+1); + +%=================================== +% subsample K to obtain the +% final result +%=================================== +Gj = K(1:2:end,1:2:end); \ No newline at end of file diff --git a/src/utilities/uwit/clahs.m b/src/utilities/uwit/clahs.m new file mode 100644 index 0000000000000000000000000000000000000000..d260026ab0d870fbddcc2dd62dcffa411bb19bd6 --- /dev/null +++ b/src/utilities/uwit/clahs.m @@ -0,0 +1,396 @@ +function CLAHSImage = clahs(Image,NrY,NrX,varargin) +%CLAHS Contrast Limited Adaptive Histogram Specification +% CLAHS locally enhances the contrast of images by +% transforming the values of each subregion, so that the +% histogram of the output subregion approximately matches a +% specified histogram. Bilinear interpolation is performed +% across neighboring subregions to eliminate artifically +% induced boundaries. +% +% J = CLAHS(I,NRY,NRX) subdivides the image into NRY*NRX +% subregions. Each subregion is then contrast limited +% histogram equalized so that the histogram of each output +% subregion approximately matches a Rayleigh distribution +% with parameter alpha = 0.4. The default histogram bin +% size and the default number of grey levels are each 256 +% for a unit8 image and 65536 for a unit16 image. +% A normalized contrast clip limit of 5 is default. The +% clipped pixels are uniformly distributed over the clipped +% histogram. +% +% The I,NRY,NRX triple can be followed by parameter or +% parameter/value pairs to specify additional arguments. +% The following parameters, parameter/value arguments may +% be specified: +% Paramter/Value Comment +% ------------------------------------------------------- +% 'Uniform' Uniform distribution. +% 'Exponential',value Exponential with parameter 0 < alpha < 1 +% 'Rayleigh',value Ralyleigh with parameter 0 3 + done = 0; + ii = 1; + while ~done + switch lower(varargin{ii}) + case 'uniform'; heq_type = 1; ii = ii+1; + case 'exponential'; heq_type = 2; alpha = varargin{ii+1}; ii = ii+2; + case 'rayleigh'; heq_type = 3; alpha = varargin{ii+1}; ii = ii+2; + case 'nrbins'; NrBins = varargin{ii+1}; ii=ii+2; + case 'cliplimit'; ClipLimit = varargin{ii+1}; ii=ii+2; + case 'nrgreylevels';NrGreyLevels = varargin{ii+1}; ii=ii+2; + case 'simple'; clip_type = HEQ_CLIP_SIMPLE; ii=ii+1; + case 'redistribute';clip_type = HEQ_CLIP_REDISTRIBUTE; ii=ii+1; + case 'preserveminmax'; Min = min(I(:)); Max = max(I(:)); ii=ii+1; + otherwise + msg = sprintf('Unknown argument given'); + error(msg); + end + if ii >= length(varargin); done = 1; end + end +end + + +status = 0; +%====================================== +% various sanity checks on the inputs +%====================================== +if NrX > MAX_REG_X + msg = sprintf('NrX > MAX_REG_X = %g',MAX_REG_X); + error(msg); +end +if NrY > MAX_REG_Y + msg = sprintf('NrY > MAX_REG_Y = %g',MAX_REG_Y); + error(msg); +end +if mod(XRes,NrX) + msg = sprintf('XRes/NrX = %g/%g = %g, noninteger value', ... + XRes,NrX,XRes/NrX); + error(msg); +end +if mod(YRes,NrY) + msg = sprintf('YRes/NrY = %g/%g = %g, noninteger value', ... + YRes,NrY,YRes/NrY); + error(msg); +end +if Max > NrGreyLevels + msg = sprintf('Max image value %g > number of greylevels %g', ... + Max,NrGreyLevels); + error(msg); +end +if (NrX <2 | NrY <2) + msg = sprintf('NrX < 2 or NrY < 2, number of subregions must be >= 4'); + error(msg); +end +if ClipLimit <= 1 + msg = sprintf('ClipLimit <= 1, performing standard AHE ...'); + disp(msg); +elseif isa(Image,'uint16') + ClipLimit = ClipLimit*256; +end + +%============================== +% setup/initalization +%============================== +% create an array to hold the histogram for each subregion +MapArray = zeros(NrX*NrY*NrBins,1); + +% determine properties of each subregion +XSize = XRes/NrX; +YSize = YRes/NrY; +NrPixels = XSize*YSize; + +% calculate actual clip limit +if ClipLimit > 1 + ClipLimit = (ClipLimit*NrPixels/NrBins); + if ClipLimit < 1 + ClipLimit = 1; + else + ClipLimit = ClipLimit; + end +else + % large value, do not clip (AHE) + ClipLimit = inf; + clip_type = 0; +end + +%================================================== +% calculate greylevel mappings for each subregion +%================================================== +% calculate and process histograms for each subregion +for Y = 0:(NrY-1) + for X = 0:(NrX-1) + Xtmp = X*XSize+1; + Ytmp = Y*YSize+1; + indX = Xtmp:Xtmp+XSize-1; + indY = Ytmp:Ytmp+YSize-1; + SubRegion = I(indY,indX); + if NrBins >= NrGreyLevels + Hist = histc(SubRegion(:),[0:NrBins-1]); + else + Hist = histc(SubRegion(:),linspace(0,NrGreyLevels,NrBins)); + end + %figure(1); bar(Hist); + + % clip histogram, simple or redistribute depending on input parameters + if clip_type == HEQ_CLIP_REDISTRIBUTE + Hist = ClipHistogram(Hist,NrBins,ClipLimit); + elseif clip_type == HEQ_CLIP_SIMPLE + Hist = ClipHistogramSimple(Hist,NrBins,ClipLimit); + end + + %figure(2);bar(Hist); + % create histogram mapping (uniform,exponential,or rayleigh) + Hist = MapHistogram(Hist, Min, Max, NrBins, NrPixels, heq_type, alpha); + %figure(3);bar(Hist); pause; + % write working histogram into appropriate part of MapArray + MapArray((NrBins *(Y*NrX+X))+1:(NrBins *(Y*NrX+X))+NrBins) = Hist; + end +end + +%==================================================== +% interpolate greylevel mappings to get CLAHE image +%==================================================== +% make lookup table for mapping of grey values +LUT = zeros(NrGreyLevels,1); +LUT = MakeLUT(LUT, Min, Max, NrBins); +CLAHSImage = zeros(YRes,XRes); +lenY = 1; +for Y = 0:NrY + if Y == 0 %special case top row + SubY = floor(YSize/2); YU = 0; YB = 0; + elseif Y == NrY %special case bottom row + SubY = floor(YSize/2); YU = NrY-1; YB = YU; + else + SubY = YSize; YU = Y-1; YB = YU+1; + end + indY = lenY:lenY+SubY-1; + lenX = 1; + for X = 0:NrX + if X==0 %special case Left column + SubX = floor(XSize/2); XL = 0; XR = 0; + elseif X == NrX %special case right column + SubX = floor(XSize/2); XL = NrX-1; XR = XL; + else + SubX = XSize; XL = X-1; XR = XL+1; + end + + % retrieve the appropriate histogram mappings from MapArray + LU = MapArray((NrBins*(YU*NrX +XL))+1:((NrBins*(YU*NrX +XL)))+NrBins); + RU = MapArray((NrBins*(YU*NrX +XR))+1:((NrBins*(YU*NrX +XR)))+NrBins); + LB = MapArray((NrBins*(YB*NrX +XL))+1:((NrBins*(YB*NrX +XL)))+NrBins); + RB = MapArray((NrBins*(YB*NrX +XR))+1:((NrBins*(YB*NrX +XR)))+NrBins); + + % interpolate the appropriate subregion + indX = lenX:lenX+SubX-1; + SubRegion = I(indY,indX); + InterpD = Interpolate(SubRegion,LU,RU,LB,RB,SubX,SubY,LUT); + CLAHSImage(indY,indX) = InterpD; + lenX = lenX+SubX; + end + lenY = lenY+SubY; +end + +%============================================================= +% output the CLAHEImage in the same format as the input image +%============================================================= +if isa(Image,'uint16'); CLAHSImage = uint16(CLAHSImage); +else; CLAHSImage = uint8(CLAHSImage); +end +%===================================================================================== + + + +%================================== +% Private Subfunctions +%================================== + +%===================================================================================== +function LUT = MakeLUT(LUT,Min,Max,NrBins) +% To speed up histogram clipping, the input image [Min,Max] is scaled down to +% [0,NrBins-1]. This function calculates the LUT. +BinSize = 1 + floor((Max-Min)/NrBins); +i = (Min+1):(Max+1); +LUT(i) = min(1 + floor((i - Min)/BinSize),NrBins); +%===================================================================================== + +%===================================================================================== +function NewHistogram = ClipHistogram(Histogram,NrGreyLevels,ClipLimit) +% This function performs clipping of the histogram and redistribution of the +% clipped bin elements. Histogram is clipped and excess elements are counted +% Then excess elements are equally reditributed over the whole histogram +% (providing the bin count is smaller than the cliplimit) + +% number of excess pixels created by clipping +NrExcess = sum(max(Histogram-ClipLimit,0)); + +%==================================================================== +% clip Histogram and redistribute excess bin elements in each bin +%==================================================================== +% # of elements to be redist'ed to each bin +BinIncr = floor(NrExcess/NrGreyLevels); +% max bin value where redist. will be above Climit +Upper = ClipLimit - BinIncr; + +% clip the histogram to ClipLimit +Histogram = min(Histogram,ClipLimit); +% add partial BinIncr pixels to bins up to ClipLimit +ii = find(Histogram > Upper); +NrExcess = NrExcess - sum(ClipLimit - Histogram(ii)); +Histogram(ii) = ClipLimit; +% add BinIncr to all other bins +jj = find(Histogram <= Upper); +NrExcess = NrExcess - length(jj)*BinIncr; +Histogram(jj) = Histogram(jj) + BinIncr; + +% evenly redistribute remaining excess pixels +while NrExcess>0 + h = 1; + while (h < NrGreyLevels & NrExcess > 0) + % choose step to distribute the most excess evenly in one pass + StepSize = ceil(NrGreyLevels/NrExcess); + i = h; + while (i<(NrGreyLevels+1) & NrExcess >0), + if Histogram(i) < ClipLimit + Histogram(i) = Histogram(i) + 1; + NrExcess = NrExcess - 1; + end + i = i + StepSize; % step along the histogram + end + h = h + 1; % avoid concentrating pixels in bin 1 + end +end +NewHistogram = Histogram; +%===================================================================================== + +%===================================================================================== +function NewHistogram = ClipHistogramSimple(Histogram,NrGreyLevels,ClipLimit) +% This function performs clipping of the histogram +% any bin with a value above the cliplimit is assigned the value of ClipLimit + +NewHistogram = min(Histogram,ClipLimit); +%===================================================================================== + + +%===================================================================================== +function Histogram = MapHistogram(Histogram, Min, Max, NrGreyLevels, NrofPixels, heq_type, heq_alpha) +% This function calculates the equalized lookup table (mapping) +% by cumulating the input histogram +% Note: Lookup table is rescaled in the range [Min..Max]. + +HEQ_NONE = 0; +HEQ_UNIFORM = 1; +HEQ_EXPONENTIAL = 2; +HEQ_RAYLEIGH = 3; + +Scale = (Max-Min)/NrofPixels; + +switch heq_type +case HEQ_UNIFORM, %accummulate histogram uniformly + Sum = cumsum(Histogram); + % scale it from min to max + Histogram = min(Min + Sum*Scale, Max); %limit range to Max +case HEQ_EXPONENTIAL, %accumulate histogram exponentially + Sum = cumsum(Histogram); + vmax = 1.0 - exp(-heq_alpha); + temp = -1/heq_alpha*log(1-(vmax*Sum/NrofPixels)); + Histogram = min(temp*Max,Max); %limit range to Max +case HEQ_RAYLEIGH, %accumulate histogram using rayleigh + Sum = cumsum(Histogram); + hconst = 2*heq_alpha*heq_alpha; + vmax = 1 - exp((-1/hconst)); + temp = sqrt(-hconst*log(1-vmax*(double(Sum)/NrofPixels))); + Histogram = min(temp*Max,Max); %limit range to Max +otherwise, %just do UNIFORM if heq_type has a wacky value + Sum = cumsum(Histogram); + Histogram = min(Min + Sum*Scale,Max); %limit range to Max +end +%===================================================================================== + +%===================================================================================== +function InterpRegion = Interpolate(SubRegion,MapLU,MapRU,MapLB,MapRB,XSize,YSize,LUT) +% This function calculates the new greylevel assignment of pixels within +% a subregion of the image with size XSize YSize. This is done by a bilinear +% interpolation between four different histogram mappings in order to eliminate +% boundary artifacts. + +Num = XSize * YSize; %Normalization factor +BinValues = LUT(SubRegion+1); +[XInvCoef{1:YSize,1}] = deal([XSize:-1:1]); XInvCoef = cell2mat(XInvCoef); +[YInvCoef{1,1:XSize}] = deal([YSize:-1:1]'); YInvCoef = cell2mat(YInvCoef); +[XCoef{1:YSize,1}] = deal([0:XSize-1]); XCoef = cell2mat(XCoef); +[YCoef{1,1:XSize}] = deal([0:YSize-1]'); YCoef = cell2mat(YCoef); + +InterpRegion = (YInvCoef.*(XInvCoef.*MapLU(BinValues) + XCoef.*MapRU(BinValues)) ... + + YCoef.*(XInvCoef.*MapLB(BinValues) + XCoef.*MapRB(BinValues)))/Num; +%===================================================================================== \ No newline at end of file diff --git a/src/utilities/uwit/fft_code/Blend_pair.m b/src/utilities/uwit/fft_code/Blend_pair.m new file mode 100644 index 0000000000000000000000000000000000000000..7d192aa4a8cefe5350c5ecb76118022129d2ba87 --- /dev/null +++ b/src/utilities/uwit/fft_code/Blend_pair.m @@ -0,0 +1,60 @@ + +function [I_mos] = Merge_pair(I1, I2, tform, varargin) + +%=============================================== +% CHECK FOR OPTIONAL ARGUMENTS for the mask +%=============================================== +NARGIN = nargin; +Imaskflag = 0; +if NARGIN < 3 + error('Invalid number of arguments'); +elseif NARGIN > 3 + done = 0; + ii = 1; + while ~done + switch lower(varargin{ii}) + case 'mask'; Imask = varargin{ii+1}; ii=ii+2; Imaskflag = 1; + otherwise + msg = sprintf('Unknown argument given'); + error(msg); + end + if ii >= length(varargin); done = 1; end + end +end + +if Imaskflag == 1 + I1 = immultiply(imadd(I1,1),uint8(Imask)); + I2 = immultiply(imadd(I2,1),uint8(Imask)); +end; + +%warp the input image I2 into I1's frame using +%the determined transform. calculate the warped +%image bounds so that both images are completly +%contained +inbounds = [1 1; size(I1,2) size(I1,1)]; +outbounds = findbounds(tform,inbounds); +mosaicbounds(1,:) = min(inbounds(1,:),outbounds(1,:)); +mosaicbounds(2,:) = max(inbounds(2,:),outbounds(2,:)); +xdata = mosaicbounds(:,1)'; +ydata = mosaicbounds(:,2)'; +J2 = imtransform(I2,tform,'bicubic','xdata',xdata,'ydata',ydata); + +%place the original I1 image into the mosaic canvas +J1 = imtransform(I1,maketform('affine',eye(3)),'bicubic', ... + 'xdata',xdata,'ydata',ydata); + +R = (zeros(size(J1))); +%generate a mosaic by creating an average of the two +[ii,jj] = find((J2~=0)); +iimin = min(ii); +jjmin = min(jj); +ind_t = find(ii > iimin+12 & jj > jjmin+12); +ii = ii(ind_t); jj = jj(ind_t); +ind = sub2ind(size(J1),ii,jj); +%R(ind) = 1; +%imshow(uint8(255*R)); +%Return the mosaic as an output. + +R(300:end,:) = 1; +keyboard +I_mos = blend(J2,J1,R); \ No newline at end of file diff --git a/src/utilities/uwit/fft_code/Merge_pair.m b/src/utilities/uwit/fft_code/Merge_pair.m new file mode 100644 index 0000000000000000000000000000000000000000..6184c196237fc5f25441645d599707014769bd84 --- /dev/null +++ b/src/utilities/uwit/fft_code/Merge_pair.m @@ -0,0 +1,64 @@ +function [I_mos] = Merge_pair(I1, I2, tform, varargin) + +%=============================================== +% CHECK FOR OPTIONAL ARGUMENTS for the mask +%=============================================== +NARGIN = nargin; +Imaskflag = 0; +if NARGIN < 3 + error('Invalid number of arguments'); +elseif NARGIN > 3 + done = 0; + ii = 1; + while ~done + switch lower(varargin{ii}) + case 'mask'; Imask = varargin{ii+1}; ii=ii+2; Imaskflag = 1; + otherwise + msg = sprintf('Unknown argument given'); + error(msg); + end + if ii >= length(varargin); done = 1; end + end +end + +if Imaskflag == 1 + I1 = immultiply(imadd(I1,1),uint8(Imask)); + I2 = immultiply(imadd(I2,1),uint8(Imask)); +end; + +%warp the input image I2 into I1's frame using +%the determined transform. calculate the warped +%image bounds so that both images are completly +%contained +inbounds = [1 1; size(I1,2) size(I1,1)]; +outbounds = findbounds(tform,inbounds); +mosaicbounds(1,:) = min(inbounds(1,:),outbounds(1,:)); +mosaicbounds(2,:) = max(inbounds(2,:),outbounds(2,:)); +xdata = mosaicbounds(:,1)'; +ydata = mosaicbounds(:,2)'; +J2 = imtransform(I2,tform,'bicubic','xdata',xdata,'ydata',ydata); + +%place the original I1 image into the mosaic canvas +J1 = imtransform(I1,maketform('affine',eye(3)),'bicubic', ... + 'xdata',xdata,'ydata',ydata); + +%generate a mosaic by creating an average of the two +ind = find((J1~=0) & (J2~=0)); +Mavg = double(J1)+double(J2); +Mavg(ind) = Mavg(ind)/2; +Mavg = uint8(Mavg); + +figure(1); clf; +imagesc(I1); axis on; colormap gray; +title('Control Image'); + +figure(2); clf; +imagesc(I2); axis on; colormap gray; +title('Input Image'); + +figure(3); clf; +imshow(xdata,ydata,Mavg); axis on; +title('Registered Input Image (Average Intensity)'); + +%Return the mosaic as an output. +I_mos = Mavg; diff --git a/src/utilities/uwit/fft_code/coswin.m b/src/utilities/uwit/fft_code/coswin.m new file mode 100644 index 0000000000000000000000000000000000000000..b28af40f03632c05b75449b31e21b7232d774573 --- /dev/null +++ b/src/utilities/uwit/fft_code/coswin.m @@ -0,0 +1,18 @@ +function cwin = coswin(rdim,cdim,support) +% cosine window that smooths out borders of image before filtering +% +% History: +% Date Who What +%-------- ------- --------------------------------- +% 04/2002 RE Touched. +% 05/2000 OP Create. + +n = [0:support-1]; +cosseq = 0.5*(cos(pi*n/support+pi)+1); + +cwin = ones(rdim,cdim); +cwin(1:support,:)=cosseq'*ones(1,cdim); +cwin(rdim-support+1:rdim,:)=flipud(cosseq')*ones(1,cdim); +cwin(:,1:support) = cwin(:,1:support).*(ones(rdim,1)*cosseq); +cwin(:,cdim-support+1:cdim) = cwin(:,cdim-support+1:cdim).* ... + (ones(rdim,1)*fliplr(cosseq)); \ No newline at end of file diff --git a/src/utilities/uwit/fft_code/do_demo.m b/src/utilities/uwit/fft_code/do_demo.m new file mode 100644 index 0000000000000000000000000000000000000000..e4fde644185bbc2e5e002df540575c19d18a74a8 --- /dev/null +++ b/src/utilities/uwit/fft_code/do_demo.m @@ -0,0 +1,8 @@ +if 1 %S + I1 = imread('./images/fourier_control.tif'); + I2 = imread('./images/fourier_input.tif'); + [scale,theta,t] = fftreg(I1,I2); + + tform = tform_from_similarity(scale,theta,t,(size(I1)-1)./2); + Imos = Merge_pair(I1,I2,tform); +end diff --git a/src/utilities/uwit/fft_code/fftreg.m b/src/utilities/uwit/fft_code/fftreg.m new file mode 100644 index 0000000000000000000000000000000000000000..deb7d22e47aaf522a12e68eb6bbd241a46bd4db9 --- /dev/null +++ b/src/utilities/uwit/fft_code/fftreg.m @@ -0,0 +1,502 @@ +function [varargout] = fftreg(I1,I2,varargin) +%FFTREG Registers an image pair for rotation, scale, and +% and tranlation using Fourier transform properties. +% +% [SCALE,THETA,TRAN] = FFTREG(I1,I2) registers a pair of images +% I1,I2 using Fourier based methods which can account for images +% which are related through a scaling, rotation, and translation. +% The parameters SCALE,THETA, and TRAN are all measured relative +% to I1's coordinate frame. TRAN is a 2x1 vector of translations +% [tu tv]'. +% +% [SCALE,THETA,TRAN,J] = FFTREG(I1,I2) and +% [J] = FFTREG(I1,I2) optionally returns the registered +% output image I2 in I1's coordinate frame. +% +% FFTREG(I1,I2) without any output argument assignments generates +% output figures only. +% +% The I1,I2 pair can be followed by parameter/value pairs to +% specify additional arguments. The following parameter/value +% arguments may be specified (not case sensitive): +% Paramter/Value Comment +% ------------------------------------------------------- +% 'F1DIM' # of samples of FFT in column direction +% 'F2DIM' # of samples of FFT in row direction +% 'RDIM' # of samples in log-polar radial coordinate +% 'TDIM' # of samples in log-polar theta coordinate +% 'CWIN' size of cosine window support +% 'THRESH' controls oversampling of pixels with small +% radial component, default 600 +% 'SHOW_FIGS' 0 produces no figures +% 1 produces only registered output image figures +% 2 produces lots of figures +% +%EXAMPLE1 +% +% I1 = rgb2gray(imread('westconcordaerial.png')); +% [rdim,cdim] = size(I1); +% scale = 1.2; +% theta = 25*pi/180; +% t = [-15 -15]; +% H12 = eye(3); +% R12 = [cos(theta) -sin(theta); sin(theta) cos(theta)]; +% H12(1:2,1:2) = (1/scale)*R12; +% H12(1:2,3) = t'; +% H21 = H12^-1; +% cntr = (size(I1)-1)./2; +% xo = cntr(2); yo = cntr(1); +% Htl = [1 0 xo; 0 1 yo; 0 0 1]; +% tform = maketform('affine',(Htl*H21*Htl^-1)'); +% xdata = [1 size(I1,2)]; +% ydata = [1 size(I1,1)]; +% I2 = imtransform(I1,tform,'bicubic','xdata',xdata,'ydata',ydata); +% fftreg(I1,I2); +% +%EXAMPLE2 +% +% I1 = imread('images/ESC.0565.tif'); +% I2 = imread('images/ESC.0564.tif'); +% fftreg(I1,I2); + +%HISTORY +%DATE WHO COMMENT +%-------- ------------- ----------------------------- +%10/2002 Ryan Eustice Changed to use imtransform during +% calculation of translation instead +% of imrotate & imresize. +%10/2002 Ryan Eustice Changed to use Matlab's imtransform +% function to apply the homography which +% registers the image pair. +%10/2002 Ryan Eustice Changed function name from rstreg to fftreg +%05/2002 Ryan Eustice Streamlined and speed up original code. +%05/2000 Oscar Pizarro Originally created and implemented +% all functionality of algorithm. + + +show_figs = 0; +Imaskflag = 0; +%=============================================== +% CHECK FOR OPTIONAL ARGUMENTS +%=============================================== +NARGIN = nargin; +if NARGIN < 2 + error('Invalid number of arguments'); +elseif NARGIN > 2 + done = 0; + ii = 1; + while ~done + switch lower(varargin{ii}) + case 'f1dim'; f1dim = varargin{ii+1}; ii=ii+2; + case 'f2dim'; f2dim = varargin{ii+1}; ii=ii+2; + case 'rdim'; rdim = varargin{ii+1}; ii=ii+2; + case 'tdim'; tdim = varargin{ii+1}; ii=ii+2; + case 'cwin'; cwin = varargin{ii+1}; ii=ii+2; + case 'thresh'; thresh = varargin{ii+1}; ii=ii+2; + case 'show_figs'; show_figs = varargin{ii+1}; ii=ii+2; + case 'mask'; Imask = varargin{ii+1}; ii=ii+2; Imaskflag = 1; + otherwise + msg = sprintf('Unknown argument given'); + error(msg); + end + if ii >= length(varargin); done = 1; end + end +end + +NARGOUT = nargout; +if NARGOUT == 0; + show_figs = 2; +end + + +%==================================== +% CREATE COSINE WINDOW THAT BRINGS +% INTENSITIES TO ZERO ON IMAGE BORDERS +%==================================== +[rowdim,coldim] = size(I1); +if ~exist('cwin','var') + %set default size of support + cos_support = round(0.1*min(rowdim,coldim)); +end +cwin = coswin(rowdim,coldim,cos_support); + + +%============================== +% I1 CONTROL IMAGE +%============================== +if show_figs >= 1 + figure(1) + imagesc(I1); colormap gray; axis image; grid on; truesize; + title('Control image'); +end + +I1prime = double(I1); +I1prime = I1prime - mean(I1prime(:)); %remove DC mean intensity +I1prime = I1prime.*cwin; %smooth freq at border of image +if show_figs >= 2 + figure(2) + imagesc(I1prime); colormap gray; axis image; grid on; truesize; + title('Windowed Control image'); +end + +%=============================== +% I2 INPUT IMAGE +%=============================== +if show_figs >= 1 + figure(3) + imagesc(I2); colormap gray; axis image; grid on; truesize; + title('Input image'); +end + +I2prime = double(I2); +I2prime = I2prime - mean(I2prime(:)); %remove DC mean intensity +I2prime = I2prime.*cwin; %smooth freq at border of image +if show_figs >= 2 + figure(4) + imagesc(I2prime); colormap gray; axis image; grid on; truesize; + title('Windowed Input image'); +end + + +%================================== +% HIGH FREQ EMPHASIS FILTER +%================================== +fsize = 41; %assume odd filter support +if ~exist('sigma','var') + %set default + sigma = 1.5; %std dev in pixels +end + +%design filter's spatial impulse response +h = fspecial('log',fsize,sigma); + + +%==================================== +% APPLY FILTER TO IMAGES +%==================================== +%(f1dim,f2dim) are dimensions of FFT's +%which control zero padding and aliasing +%set defaults to be of equal dimension +if ~exist('f1dim') + f1dim = max(rowdim,coldim); %f1dim >= coldim +end +if ~exist('f2dim') + f2dim = max(rowdim,coldim); %f2dim >= rowdim +end + +%phase correction to translate filter to the origin +k1 = [0:f2dim-1]'; +k2 = [0:f1dim-1]; +phasecorrection = ... + (exp(j*k1*2*pi/(f2dim)*(fsize-1)/2)*exp(j*k2*2*pi/f1dim*(fsize-1)/2)); + +%apply laplacian of gaussian filter to images +%in the frequency domain with the correct phase +hF1 = fft2(I1prime,f2dim,f1dim); +hF2 = fft2(I2prime,f2dim,f1dim); +fH = fft2(h,f2dim,f1dim); +hF1 = hF1.*fH.*phasecorrection; +hF2 = hF2.*fH.*phasecorrection; + + +%===================================== +% MAGNITUDE OF FT OF FILTERED IMAGES +%===================================== +M1 = (fftshift(abs(hF1))); +M2 = (fftshift(abs(hF2))); +if show_figs >= 2 + figure(5); + imagesc(M1); axis image; grid on; + title('Magnitude of spectrum. Filtered Control image.'); + crange = caxis; + + figure(6); + imagesc(M2,crange); axis image; grid on; + title('Magnitude of spectrum. Filtered Input image.'); +end + + +%======================================= +% INVERSE TRANSFORM SPECTRA TO GET +% FILTERED IMAGES IN SPATIAL DOMAIN +%======================================= +hI1 = (real(ifft2(hF1))); +hI1 = hI1(1:rowdim,1:coldim); +hI2 = (real(ifft2(hF2))); +hI2 = hI2(1:rowdim,1:coldim); + +if show_figs >= 2 + figure(7); + imagesc(hI1); axis image; grid on; truesize; + title('Filtered Control image.'); + crange = caxis; + + figure(8); + imagesc(hI2,crange); axis image; grid on; truesize; + title('Filtered Input image.'); +end + +%conserve memory and clear intermediate variables +clear I1prime I2prime H phasecorrection; + + +%============================================ +% CREATE LOG-POLAR CONVERSION LOOK-UP-TABLE +% IF IT DOESN'T ALREADY EXIST +%============================================ +tol = 0.001; +Hmin = 0.03; + +%find radius of bandpass annulus of +%highpass emphasis filter by looking in +%upper left quadrant of fft +%(fft calculated w/o correction for fftshift) +wdim1 = find(Hmin-tol < abs(fH(1,1:f1dim/2)) & ... + Hmin+tol > abs(fH(1,1:f1dim/2))); +wdim2 = find(Hmin-tol < abs(fH(1:f2dim/2,1)) & ... + Hmin+tol > abs(fH(1:f2dim/2,1))); + +%keep the maximum radial dimension +wdim1 = max(wdim1); +wdim2 = max(wdim2); + +%choose the minimum of the two frequency axis +%components as the size of the frequency annulus +%to map into log-polar coordinates +wdim = min(wdim1,wdim2); + + +%(rdim,tdim) are dimensions of log-polar +%mapped image representing the number of +%samples in the (log(rho),theta) axes repectively +%set defaults to be <= min(f1dim,f2dim) +%frequency resolution in log-polar space +%cannot be any better than resolution in +%cartesian domain +if ~exist('rdim') + rdim = min(f1dim,f2dim); %# of samples in log(rho) axis +end +if ~exist('tdim') + tdim = min(f1dim,f2dim); %# of samples in theta axis +end + +%check to see if a previously cached LUT exists +%otherwise create it +lutfile = strcat(tempdir,'lut.mat'); +if exist(lutfile,'file') + load(lutfile); +end +createLUT = 0; +if exist('lut','var') + if lut.f1dim ~= f1dim | lut.f2dim ~= f2dim + createLUT = 1; + elseif lut.tdim ~= tdim | lut.rdim ~= rdim + createLUT = 1; + elseif exist('thresh','var'); + if lut.thresh ~= thresh + createLUT = 1; + end + end +else + createLUT = 1; +end +if createLUT + if exist('thresh','var'); + fprintf('Creating LUT with thresh = %g',thresh); + else + fprintf('Creating LUT ...'); + end + if exist('thresh','var') + lut = lplut(f1dim,f2dim,wdim,rdim,tdim,thresh); + else + lut = lplut(f1dim,f2dim,wdim,rdim,tdim); + end + save(strcat(tempdir,'lut.mat'),'lut'); + fprintf(' done\n\n'); +else + fprintf('Using cached LUT with thresh = %g ...\n\n',lut.thresh); +end + + +%========================================== +% MAP MAGNITUDE SPECTRUMS TO LOG-POLAR SPACE +%========================================== +N1 = (lutmapper(M1,lut.w2lcmap,lut.w1lcmap,lut.indvec)); +N2 = (lutmapper(M2,lut.w2lcmap,lut.w1lcmap,lut.indvec)); +[tdim,rdim] = size(N1); +if show_figs >= 2 + figure(9); + imagesc([],[0:tdim-1]/lut.vscale,N1); grid on; + title('Log-polar Magnitude of Spectrum. Control Image.'); + xlabel('samples of log(\rho), [\rho measured in pixels]'); + ylabel('\theta [deg]') + crange = caxis; + + figure(10); + imagesc([],[0:tdim-1]/lut.vscale,(N2),crange); grid on; + title('Log-polar Magnitude of Spectrum. Input Image.'); + xlabel('samples of log(\rho), [\rho measured in pixels]'); + ylabel('\theta [deg]') +end + + +%=========================================== +% CALCULATE SCALE AND THETA PARAMETERS VIA +% PHASE CORRELATION TECHNIQUE +% -->circular convolution in the theta direction +% -->linear convolution in the radius direction +%=========================================== +%Q1 & Q2 are the transforms of N1 & N2, +%the log-polar representations of M1 & M2 +Q1 = fft2(N1,tdim,2*rdim-1); +Q2 = fft2(N2,tdim,2*rdim-1); + +%conserve memory and clear intermediate variables +clear N1 N2 M1 M2; + +%calculate the cross-power spectrum +CPS = Q1.*conj(Q2); +CPS = CPS./abs(CPS); + +%inverse transform cross-power spectrum +%to get an impulse +peakM = real(fftshift(ifft2(CPS))); + +%conserve memory and clear intermediate variables +clear Q1 Q2 CPS; + +%find coordinates of impulse to recover +%scale and theta parameters +[yM,xM] = size(peakM); + +%maxval = max(peakM(:)); +%[iind,jind] = find(peakM == maxval); + +% Added by Ali Can. Dec.5.2002. +% Correlation matrix is smoothed before searching the maximum +% this is essential for +% 1. tolerating minor image-to-image transformation modelling errors +% 2. to reduce the effect of bias towards no translation + +hf = ones(3,3); +hf = hf ./ sum(hf(:)); +peakM_s = imfilter(peakM,hf,'same','circular'); +maxval = max(peakM_s(:)); +[iind,jind] = find(peakM_s == maxval); +%figure, plot(max(peakM)); +%figure, plot(max(peakM_s)); +%figure, my_imshow(peakM); +%figure, my_imshow(peakM_s); + +%%%%%% End of Ali's addition Dec.5.2002 + + +% refer to image coordinates +jmax = jind - (xM/2) - 1; +imax = iind - (yM/2) - 1; + + +%============================================ +% CALCULATE SCLAE AND ROTATION USING +% MAPPING PARAMETERS +%============================================ +scale = lut.base^(jmax/lut.hscale); +theta = -imax/lut.vscale; %multiply by -1 to conform to + %convention of theta defined + %positive counter-clockwise + + +%============================================= +% DETERMINE TRANSLATIONAL OFFSET AND +% RESOLVE 180 DEGREE AMBIGUITY IN THETA +%============================================= +%transform filtered input image according to theta and scale +%using MATLAB's tform structure and imtransform + +tform = tform_from_similarity(scale,theta,[0, 0],(size(I1)-1)./2); + +%warp the input image I2 using the +%determined transform. +hJ2 = imtransform(hI2,tform,'bicubic', ... + 'udata',[1 size(I2,2)],'vdata',[1 size(I2,1)], ... + 'xdata',[1 size(I1,2)],'ydata',[1 size(I1,1)]); +%rotate by 180 degrees +hJ2_180 = flipud(fliplr(hJ2)); + +% find translation using phase correlation technique +[tu1,tv1,peakcorr1] = find_xlate2(hI1,hJ2); +[tu2,tv2,peakcorr2] = find_xlate2(hI1,hJ2_180); + +% If a mask is provided, ignore 180 degree ambiguity +if Imaskflag == 1 + tu = tu1; + tv = tv1; + peakcorr = peakcorr1; +else + if peakcorr1 > peakcorr2 + tu = tu1; + tv = tv1; + peakcorr = peakcorr1; + else + tu = tu2; + tv = tv2; + peakcorr = peakcorr2; + %theta = theta+180; + end +end; + +%conserve memory and clear intermediate variables +clear tu1 tv1 peakcorr1 tu2 tv2 peakcorr2 hI2 hJ2_180; + + +%Resolve the periodicity of translation by a hypoyhesize-test scheme +if Imaskflag == 1 + [tu, tv] = resolve_xlate2(I1, I2, scale,theta,[tu, tv],(size(I1)-1)./2,'mask',Imask); +else + [tu, tv] = resolve_xlate2(I1, I2, scale,theta,[tu, tv],(size(I1)-1)./2); +end; + +%display results +fprintf('scale = %g, res = %g, rdim = %g samples ...\n', ... + scale,lut.base^(1/lut.hscale)-1,rdim); +fprintf('theta = %g degrees, res = %g, tdim = %g samples ...\n', ... + theta,1/lut.vscale,tdim); +fprintf('translation (x,y) = (%g,%g) ...\n',tu,tv); + + +%============================================= +% CREATE A MATLAB TFORM STRUCTURE WHICH +% CAN BE USED BY IMTRANSFORM TO WARP +% THE INPUT IMAGE +%============================================= + +tform = tform_from_similarity(scale,theta,[tu, tv],(size(I1)-1)./2); + +%register the input image I2 using the +%determined transform into I1's coordinate frame +J2 = imtransform(I2,tform,'bicubic', ... + 'udata',[1 size(I2,2)],'vdata',[1 size(I2,1)], ... + 'xdata',[1 size(I1,2)],'ydata',[1 size(I1,1)]); + +if show_figs >= 1 + figure(12); + imagesc(J2); colormap gray; axis on; grid; truesize; + title('Registered Input Image'); +end + +%========================================== +% ASSIGN OUTPUT ARGUMENTS +%========================================== +if NARGOUT > 0 + if NARGOUT == 1 + varargout{1} = J2; + end + if NARGOUT >= 3 + varargout{1} = scale; + varargout{2} = theta; + varargout{3} = [tu tv]; + end + if NARGOUT >= 4 + varargout{4} = J2; + end +end \ No newline at end of file diff --git a/src/utilities/uwit/fft_code/fftreg.m.orig b/src/utilities/uwit/fft_code/fftreg.m.orig new file mode 100644 index 0000000000000000000000000000000000000000..1b0acc64e568d76387dc214dd19643ce6ac9efad --- /dev/null +++ b/src/utilities/uwit/fft_code/fftreg.m.orig @@ -0,0 +1,485 @@ +function [varargout] = fftreg(I1,I2,varargin) +%FFTREG Registers an image pair for rotation, scale, and +% and tranlation using Fourier transform properties. +% +% [SCALE,THETA,TRAN] = FFTREG(I1,I2) registers a pair of images +% I1,I2 using Fourier based methods which can account for images +% which are related through a scaling, rotation, and translation. +% The parameters SCALE,THETA, and TRAN are all measured relative +% to I1's coordinate frame. TRAN is a 2x1 vector of translations +% [tu tv]'. +% +% [SCALE,THETA,TRAN,J] = FFTREG(I1,I2) and +% [J] = FFTREG(I1,I2) optionally returns the registered +% output image I2 in I1's coordinate frame. +% +% FFTREG(I1,I2) without any output argument assignments generates +% output figures only. +% +% The I1,I2 pair can be followed by parameter/value pairs to +% specify additional arguments. The following parameter/value +% arguments may be specified (not case sensitive): +% Paramter/Value Comment +% ------------------------------------------------------- +% 'F1DIM' # of samples of FFT in column direction +% 'F2DIM' # of samples of FFT in row direction +% 'RDIM' # of samples in log-polar radial coordinate +% 'TDIM' # of samples in log-polar theta coordinate +% 'CWIN' size of cosine window support +% 'THRESH' controls oversampling of pixels with small +% radial component, default 600 +% 'SHOW_FIGS' 0 produces no figures +% 1 produces only registered output image figures +% 2 produces lots of figures +% +%EXAMPLE1 +% +% I1 = double(rgb2gray(imread('westconcordaerial.png'))); +% [rdim,cdim] = size(I1); +% I2 = imresize(I1,1.2,'bicubic'); +% I2 = imrotate(I2,25.0,'bicubic','crop'); +% I2 = I2(1:rdim,1:cdim); +% fftreg(I1,I2); +% +%EXAMPLE2 +% +% I1 = double(imread('images/ESC.0565.tif')); +% I2 = double(imread('images/ESC.0564.tif')); +% fftreg(I1,I2); + +%HISTORY +%DATE WHO COMMENT +%-------- ------------- ----------------------------- +%10/2002 Ryan Eustice Changed function name from rstreg to fftreg +%05/2002 Ryan Eustice Streamlined and speed up original code. +%05/2000 Oscar Pizarro Originally created and implemented +% all functionality of algorithm. + + +show_figs = 0; +%=============================================== +% CHECK FOR OPTIONAL ARGUMENTS +%=============================================== +NARGIN = nargin; +if NARGIN < 2 + error('Invalid number of arguments'); +elseif NARGIN > 2 + done = 0; + ii = 1; + while ~done + switch lower(varargin{ii}) + case 'f1dim'; f1dim = varargin{ii+1}; ii=ii+2; + case 'f2dim'; f2dim = varargin{ii+1}; ii=ii+2; + case 'rdim'; rdim = varargin{ii+1}; ii=ii+2; + case 'tdim'; tdim = varargin{ii+1}; ii=ii+2; + case 'cwin'; cwin = varargin{ii+1}; ii=ii+2; + case 'thresh'; thresh = varargin{ii+1}; ii=ii+2; + case 'show_figs'; show_figs = varargin{ii+1}; ii=ii+2; + otherwise + msg = sprintf('Unknown argument given'); + error(msg); + end + if ii >= length(varargin); done = 1; end + end +end + +NARGOUT = nargout; +if NARGOUT == 0; + show_figs = 2; +end + + +%==================================== +% CREATE COSINE WINDOW THAT BRINGS +% INTENSITIES TO ZERO ON IMAGE BORDERS +%==================================== +[rowdim,coldim] = size(I1); +if ~exist('cwin','var') + %set default size of support + cos_support = 80; +end +cwin = coswin(rowdim,coldim,cos_support); + + +%============================== +% I1 CONTROL IMAGE +%============================== +if show_figs >= 1 + figure(1) + imagesc(I1); colormap gray; axis image; grid on; truesize; + title('Control image'); +end + +I1prime = double(I1); +I1prime = I1prime - mean(I1prime(:)); %remove DC mean intensity +I1prime = I1prime.*cwin; %smooth freq at border of image +if show_figs >= 2 + figure(2) + imagesc(I1prime); colormap gray; axis image; grid on; truesize; + title('Windowed Control image'); +end + +%=============================== +% I2 INPUT IMAGE +%=============================== +if show_figs >= 1 + figure(3) + imagesc(I2); colormap gray; axis image; grid on; truesize; + title('Input image'); +end + +I2prime = double(I2); +I2prime = I2prime - mean(I2prime(:)); %remove DC mean intensity +I2prime = I2prime.*cwin; %smooth freq at border of image +if show_figs >= 2 + figure(4) + imagesc(I2prime); colormap gray; axis image; grid on; truesize; + title('Windowed Input image'); +end + + +%================================== +% HIGH FREQ EMPHASIS FILTER +%================================== +fsize = 41; %assume odd filter support +if ~exist('sigma','var') + %set default + sigma = 1.5; %std dev in pixels +end + +%design filter's spatial impulse response +h = fspecial('log',fsize,sigma); + + +%==================================== +% APPLY FILTER TO IMAGES +%==================================== +%(f1dim,f2dim) are dimensions of FFT's +%which control zero padding and aliasing +%set defaults to be of equal dimension +if ~exist('f1dim') + f1dim = max(rowdim,coldim); %f1dim >= coldim +end +if ~exist('f2dim') + f2dim = max(rowdim,coldim); %f2dim >= rowdim +end + +%phase correction to translate filter to the origin +k1 = [0:f2dim-1]'; +k2 = [0:f1dim-1]; +phasecorrection = ... + (exp(j*k1*2*pi/(f2dim)*(fsize-1)/2)*exp(j*k2*2*pi/f1dim*(fsize-1)/2)); + +%apply laplacian of gaussian filter to images +%in the frequency domain with the correct phase +hF1 = fft2(I1prime,f2dim,f1dim); +hF2 = fft2(I2prime,f2dim,f1dim); +fH = fft2(h,f2dim,f1dim); +hF1 = hF1.*fH.*phasecorrection; +hF2 = hF2.*fH.*phasecorrection; + + +%===================================== +% MAGNITUDE OF FT OF FILTERED IMAGES +%===================================== +M1 = (fftshift(abs(hF1))); +M2 = (fftshift(abs(hF2))); +if show_figs >= 2 + figure(5); + imagesc(M1); axis image; grid on; + title('Magnitude of spectrum. Filtered Control image.'); + crange = caxis; + + figure(6); + imagesc(M2,crange); axis image; grid on; + title('Magnitude of spectrum. Filtered Input image.'); +end + + +%======================================= +% INVERSE TRANSFORM SPECTRA TO GET +% FILTERED IMAGES IN SPATIAL DOMAIN +%======================================= +if NARGOUT == 0 | show_figs == 2 + hI1 = (real(ifft2(hF1))); + hI1 = hI1(1:rowdim,1:coldim); + hI2 = (real(ifft2(hF2))); + hI2 = hI2(1:rowdim,1:coldim); +end +if show_figs >= 2 + figure(7); + imagesc(hI1); axis image; grid on; truesize; + title('Filtered Control image.'); + crange = caxis; + + figure(8); + imagesc(hI2,crange); axis image; grid on; truesize; + title('Filtered Input image.'); +end + +%conserve memory and clear intermediate variables +clear I1prime I2prime H phasecorrection; + + +%============================================ +% CREATE LOG-POLAR CONVERSION LOOK-UP-TABLE +% IF IT DOESN'T ALREADY EXIST +%============================================ +tol = 0.001; +Hmin = 0.03; + +%find radius of bandpass annulus of +%highpass emphasis filter by looking in +%upper left quadrant of fft +%(fft calculated w/o correction for fftshift) +wdim1 = find(Hmin-tol < abs(fH(1,1:f1dim/2)) & ... + Hmin+tol > abs(fH(1,1:f1dim/2))); +wdim2 = find(Hmin-tol < abs(fH(1:f2dim/2,1)) & ... + Hmin+tol > abs(fH(1:f2dim/2,1))); + +%keep the maximum radial dimension +wdim1 = max(wdim1); +wdim2 = max(wdim2); + +%choose the minimum of the two frequency axis +%components as the size of the frequency annulus +%to map into log-polar coordinates +wdim = min(wdim1,wdim2); + + +%(rdim,tdim) are dimensions of log-polar +%mapped image representing the number of +%samples in the (log(rho),theta) axes repectively +%set defaults to be <= min(f1dim,f2dim) +%frequency resolution in log-polar space +%cannot be any better than resolution in +%cartesian domain +if ~exist('rdim') + rdim = min(f1dim,f2dim); %# of samples in log(rho) axis +end +if ~exist('tdim') + tdim = min(f1dim,f2dim); %# of samples in theta axis +end + +%check to see if a previously cached LUT exists +%otherwise create it +lutfile = strcat(tempdir,'lut.mat'); +if exist(lutfile,'file') + load(lutfile); +end +createLUT = 0; +if exist('lut','var') + if lut.f1dim ~= f1dim | lut.f2dim ~= f2dim + createLUT = 1; + elseif lut.tdim ~= tdim | lut.rdim ~= rdim + createLUT = 1; + elseif exist('thresh','var'); + if lut.thresh ~= thresh + createLUT = 1; + end + end +else + createLUT = 1; +end +if createLUT + if exist('thresh','var'); + fprintf('Creating LUT with thresh = %g',thresh); + else + fprintf('Creating LUT ...'); + end + if exist('thresh','var') + lut = lplut(f1dim,f2dim,wdim,rdim,tdim,thresh); + else + lut = lplut(f1dim,f2dim,wdim,rdim,tdim); + end + save(strcat(tempdir,'lut.mat'),'lut'); + fprintf(' done\n\n'); +else + fprintf('Using cached LUT with thresh = %g ...\n\n',lut.thresh); +end + + +%========================================== +% MAP MAGNITUDE SPECTRUMS TO LOG-POLAR SPACE +%========================================== +N1 = (lutmapper(M1,lut.w2lcmap,lut.w1lcmap,lut.indvec)); +N2 = (lutmapper(M2,lut.w2lcmap,lut.w1lcmap,lut.indvec)); +[tdim,rdim] = size(N1); +if show_figs >= 2 + figure(9); + imagesc([],[0:tdim-1]/lut.vscale,N1); grid on; + title('Log-polar Magnitude of Spectrum. Control Image.'); + xlabel('samples of log(\rho), [\rho measured in pixels]'); + ylabel('\theta [deg]') + crange = caxis; + + figure(10); + imagesc([],[0:tdim-1]/lut.vscale,(N2),crange); grid on; + title('Log-polar Magnitude of Spectrum. Input Image.'); + xlabel('samples of log(\rho), [\rho measured in pixels]'); + ylabel('\theta [deg]') +end + + +%=========================================== +% CALCULATE SCALE AND THETA PARAMETERS VIA +% PHASE CORRELATION TECHNIQUE +% -->circular convolution in the theta direction +% -->linear convolution in the radius direction +%=========================================== +%Q1 & Q2 are the transforms of N1 & N2, +%the log-polar representations of M1 & M2 +Q1 = fft2(N1,tdim,2*rdim-1); +Q2 = fft2(N2,tdim,2*rdim-1); + +%conserve memory and clear intermediate variables +clear N1 N2 M1 M2; + +%calculate the cross-power spectrum +CPS = Q1.*conj(Q2); +CPS = CPS./abs(CPS); + +%inverse transform cross-power spectrum +%to get an impulse +peakM = real(fftshift(ifft2(CPS))); + +%conserve memory and clear intermediate variables +clear Q1 Q2 CPS; + +%find coordinates of impulse to recover +%scale and theta parameters +[yM,xM] = size(peakM); +maxval = max(peakM(:)); +[iind,jind] = find(peakM == maxval); + +% refer to image coordinates +jmax = jind - (xM/2) - 1; +imax = iind - (yM/2) - 1; + + +%============================================ +% CALCULATE SCLAE AND ROTATION USING +% MAPPING PARAMETERS +%============================================ +scale = lut.base^(jmax/lut.hscale); +theta = -imax/lut.vscale; %multiply by -1 to conform to + %convention of theta defined + %positive counter-clockwise + + +%============================================= +% DETERMINE TRANSLATIONAL OFFSET AND +% RESOLVE 180 DEGREE AMBIGUITY IN THETA +%============================================= +%transform filtered input image according to theta and scale +hJ2 = imrotate(hI2,-theta,'bilinear','crop'); +hJ2 = imresize(hJ2,1/scale,'bilinear'); +hJ2flip = flipud(fliplr(hJ2)); + +% find translation using phase correlation technique +[tu1,tv1,peakcorr1] = find_xlate2(hI1,hJ2); +[tu2,tv2,peakcorr2] = find_xlate2(hI1,hJ2flip); +if peakcorr1 > peakcorr2 + tu = tu1; + tv = tv1; + peakcorr = peakcorr1; +else + tu = tu2; + tv = tv2; + peakcorr = peakcorr2; + theta = theta+180; +end + +%conserve memory and clear intermediate variables +clear tu1 tv1 peakcorr1 tu2 tv2 peakcorr2 hI2 hJ2flip; + +%display results +fprintf('scale = %g, res = %g, rdim = %g samples ...\n', ... + scale,lut.base^(1/lut.hscale)-1,rdim); +fprintf('theta = %g degrees, res = %g, tdim = %g samples ...\n', ... + theta,1/lut.vscale,tdim); +fprintf('translation (x,y) = (%g,%g) ...\n',tu,tv); + + +%============================================= +% CREATE A MATLAB TFORM STRUCTURE WHICH +% CAN BE USED BY IMTRANSFORM TO WARP +% THE INPUT IMAGE +%============================================= +%calculate a pre-multiply homography H12 which +%rotates and scales I2 towards I1 +D2R = pi/180; +H12 = eye(3); +H12(1:2,1:2) = (1/scale)*[cos(theta*D2R) -sin(theta*D2R); + sin(theta*D2R) cos(theta*D2R)]; +H12(1:2,3) = -[tu; tv]; + +%MATLAB's tform structure assumes a post-multiply +%homography so use transpose of H12 +tform = maketform('affine',H12'); + +if show_figs >= 1 + figure(11); + %warp the input image I2 using the + %determined transform. + %note that MATLAB's imtransform function + %doesn't not place the image J2 in mosaic + %space using the translation offsets + %(tu,tv). this has to be done manually. + % J2rs = imtransform(I2,tform,'bicubic'); + J2rs = imrotate(I2,-theta,'bilinear','crop'); + J2rs = imresize(J2rs,1/scale,'bilinear'); + imagesc(J2rs); colormap gray; axis on; grid; truesize; + title('Rotated and Scaled Input Image'); +end + +if show_figs >= 1 + figure(12); + %place the rotated and scaled image in + %the control image's coordinate frame + [Jy Jx] = size(J2rs); + if tv < 0 + yi = 1-tv; + yf = min(rowdim,Jy-tv); + vi = 1; + vf = min(rowdim+tv,Jy); + else + yi = 1; + yf = min(rowdim,Jy-tv); + vi = tv+1; + vf = min(Jy,rowdim+tv); + end + + if tu < 0 + xi = 1-tu; + xf = min(coldim,Jx-tu); + ui = 1; + uf = min(coldim+tu,Jx); + else + xi = 1; + xf = min(coldim,Jx-tu); + ui = tu+1; + uf = min(Jx,coldim+tu); + end + + %windows so it will match frame of control image + J2 = zeros(rowdim,coldim); + J2(yi:yf,xi:xf) = J2rs(vi:vf,ui:uf); + imagesc(J2); colormap gray; axis on; grid; truesize; + title('Registered Input Image'); +end + + +%========================================== +% ASSIGN OUTPUT ARGUMENTS +%========================================== +if NARGOUT > 0 + if NARGOUT >= 3 + varargout{1} = scale; + varargout{2} = theta; + varargout{3} = [tu tv]; + end + if NARGOUT >= 4 + varargout{4} = J2; + end +end diff --git a/src/utilities/uwit/fft_code/find_xlate2.m b/src/utilities/uwit/fft_code/find_xlate2.m new file mode 100644 index 0000000000000000000000000000000000000000..a8ca86cbe7a6f951df438f119b225f02cf6a48b6 --- /dev/null +++ b/src/utilities/uwit/fft_code/find_xlate2.m @@ -0,0 +1,58 @@ +function [tu,tv,peakcorr] = find_xlate2(I1,I2); +%FIND_XLATE2 finds relative image translation +% Uses Fourier methods to determine phase shift and thus +% image translation. + +% History: +% DATE WHO COMMENTS +%--------- ----- -------------------------------- +%12/2002 Ali Added smoothing to phase correlation matrix. +%10/2002 RME Fixed bug, function now properly returns +% translation vector in I1's coordinate frame +%05/2002 RME Minor modifications, returns shift +% as measured in I1's coordinate frame +%05/2000 OP Created and implemented +%Hanu's original +%OP presentation modifications 18-APR-2000 +%change in size of transform to insure linear convolution + +[I1_h,I1_w]=size(I1); +[I2_h,I2_w]=size(I2); + +%find the default h,w for the inputs +def_w = 2*round(max(I1_w,I2_w)/2); +def_h = 2*round(max(I1_h,I2_h)/2); + +I1_fft = fft2(I1,def_h,def_w); +I2_fft = fft2(I2,def_h,def_w); + + +%phase correlation technique +%better results than pure correlation +CPS = (I1_fft.*conj(I2_fft)); +CPS = CPS./abs(CPS); +im_xlate = real(fftshift(ifft2(CPS))); + +%peakcorr = max(im_xlate(:)); +%[ii jj] = find(im_xlate == peakcorr); + +% Added by Ali Can. Dec.5.2002. +% Phase correlation matrix is smoothed before searching the maximum +% this is essential for +% 1. tolerating minor image-to-image transformation modelling errors +% 2. to reduce the effect of bias towards no translation + +hf = ones(3,3); +hf = hf ./ sum(hf(:)); +im_xlate_s = imfilter(im_xlate,hf,'same','circular'); +peakcorr = max(im_xlate_s(:)); +[ii jj] = find(im_xlate_s == peakcorr); +%figure, plot(max(im_xlate)); +%figure, plot(max(im_xlate_s)); +%figure, my_imshow(im_xlate); +%figure, my_imshow(im_xlate_s); + +%%%%%% End of Ali's addition Dec.5.2002 + +tv = ii - def_h/2 - 1; +tu = jj - def_w/2 - 1; \ No newline at end of file diff --git a/src/utilities/uwit/fft_code/images/ESC.0118.tif b/src/utilities/uwit/fft_code/images/ESC.0118.tif new file mode 100644 index 0000000000000000000000000000000000000000..48be78921d9791457c27d9a0e6913a6c345237c3 Binary files /dev/null and b/src/utilities/uwit/fft_code/images/ESC.0118.tif differ diff --git a/src/utilities/uwit/fft_code/images/ESC.0119.tif b/src/utilities/uwit/fft_code/images/ESC.0119.tif new file mode 100644 index 0000000000000000000000000000000000000000..02257153bd02b7c1d84be6d169d268afcc69c2b6 Binary files /dev/null and b/src/utilities/uwit/fft_code/images/ESC.0119.tif differ diff --git a/src/utilities/uwit/fft_code/images/ESC.0564.tif b/src/utilities/uwit/fft_code/images/ESC.0564.tif new file mode 100644 index 0000000000000000000000000000000000000000..f2969228f0102c5cb60ba69a6380f416c52ad94c Binary files /dev/null and b/src/utilities/uwit/fft_code/images/ESC.0564.tif differ diff --git a/src/utilities/uwit/fft_code/images/ESC.0565.tif b/src/utilities/uwit/fft_code/images/ESC.0565.tif new file mode 100644 index 0000000000000000000000000000000000000000..e8d7c691db9e6cfc60976f670d1c58ced60293df Binary files /dev/null and b/src/utilities/uwit/fft_code/images/ESC.0565.tif differ diff --git a/src/utilities/uwit/fft_code/images/fourier_control.tif b/src/utilities/uwit/fft_code/images/fourier_control.tif new file mode 100644 index 0000000000000000000000000000000000000000..48be78921d9791457c27d9a0e6913a6c345237c3 Binary files /dev/null and b/src/utilities/uwit/fft_code/images/fourier_control.tif differ diff --git a/src/utilities/uwit/fft_code/images/fourier_input.tif b/src/utilities/uwit/fft_code/images/fourier_input.tif new file mode 100644 index 0000000000000000000000000000000000000000..02257153bd02b7c1d84be6d169d268afcc69c2b6 Binary files /dev/null and b/src/utilities/uwit/fft_code/images/fourier_input.tif differ diff --git a/src/utilities/uwit/fft_code/lplut.m b/src/utilities/uwit/fft_code/lplut.m new file mode 100644 index 0000000000000000000000000000000000000000..dbde709027e08963253f51a228acadcf5de3ef97 --- /dev/null +++ b/src/utilities/uwit/fft_code/lplut.m @@ -0,0 +1,149 @@ +function lut = lplut(f1dim,f2dim,wdim,rdim,tdim,varargin) +% CREATES Lookup table +% defines LUT between log-polar and cartesian coordinates +% used to map magnitude of FT in cartesian to log-polar + +% History: +% DATE WHO COMMENTS +%--------- ----- -------------------------------- +%04/24/2002 RME Modified Oscar's original p5c.m script +% into a function that maps +% a freqency annulus to log-polar rather +% than a general rectangular section of FT. +% Also rewrote FOR-LOOP as an outter product +% of two vectors improving algorithm speed +% by an order of magnitude. +% Also changed +%05/28/2002 OP includes fd1, fd2 in mapstruct, +% only saves mapstruct +%04/21/2002 OP stretches lcmap into w1,w2 vectors for faster +% assignment + + +%===================================================== +%DEFINE CONCENTRIC ANNULUS OF FREQUENCY SPECTRUM +%TO TRANSFER INTO LOG-POLAR COORDINATES. +%THE HIGHPASS EMPHASIS FILTER APPLIED IN ROTSCALE +%DEFINES THE RADIUS OF THE BANDPASS FREQUENCY ANNULUS. +%===================================================== +%wdim must be <= min(f1dim/2,f2dim/2); +%where f1dim, f2dim are the number of +%samples in vertical and horizontal axes +%of the fourier transform, repectively. +if wdim > f1dim/2 + msg = sprintf('wdim = %g > f1dim/2 = %g, setting wdim = %g', ... + wdim,f1dim/2,f1dim/2); + disp(msg); + wdim = f1dim/2; +elseif wdim > f2dim/2 + msg = sprintf('wdim = %g > f2dim/2 = %g, setting wdim = %g', ... + wdim,f2dim/2,f2dim/2); + disp(msg); + wdim = f2dim/2; +end + +%============================================ +%DEFINE DIMENSIONS OF THE LOG-POLOAR +%MAGNITUDE SPECTRUM REPRESENTATION +%============================================ +%set minimum values +% if rdim < 512 +% rdim = 512; %rho (radius samples) +% end +% if tdim < 512 +% tdim = 512; %theta (angular samples) +% end + +%============================================ +%DEFINE SOME CONSTANTS +%============================================ +%base used to calculate log of radius +%example: +% +% base^128 = 256 +% +%maps 256 rows in cartesian spectrum to +%128 rows in polar plane +base = exp(log(f2dim)/tdim); %maps f2dim rows into tdim rows + +%degrees to radians +D2R = pi/180; + +%============================================ +%DEFINE SOME PARAMETERS CONTROLING +%LOG-POLAR CONVERSION +%============================================ +%thresh together with hscale controls +%minimum radius to be mapped avoiding 'blown up' +%pixels close to origin of frequency axis. +if length(varargin) > 0 + thresh = varargin{1}; +else + %set default value of threshold + thresh = 600; +end +hscale = (rdim+thresh)/(log2(wdim-1)/log2(base)); + +%min radius mapped into log-polar coordinates +rmin = base^((thresh+1)/hscale); + +%scaling to map 180 degrees in tdim samples +vscale = tdim/180; + +%=============================================== +%CREATE LOOK-UP-TABLE +%the original block of code was written +%as nested FOR LOOPS. +%it has been rewritten more efficienty +%using vector algebra. +%=============================================== +% ORIGINAL NESTED FOR-LOOPS +% for i = 1:tdim +% theta = (i-1)/vscale; %angle +% for j = 1:rdim +% rho = base^((j+thresh)/hscale); %radius +% %cartesian coords +% w1 = round(rho*cos(theta*D2R)); +% w2 = round(rho*sin(theta*D2R)); +% %negative and positive w1, positive w2 only +% %i.e. upper half of spectra only +% if abs(w1)<=wdim & w2<=wdim & w2>=0 +% lcmap.j(i,j) = w1; %relative to center of image +% lcmap.i(i,j) = w2; %relative to center of image +% end +% end +% end + +ii = 1:tdim; +jj = 1:rdim; +theta = (ii-1)/vscale; %vector of sampled angles +rho = base.^((jj+thresh)/hscale); %vector of sampled radii + +%(w1,w2) correspond to the cartesian coordinates +%in the freq plane +w1 = cos(theta*D2R)'*rho; +w2 = sin(theta*D2R)'*rho; + +%find nearest sample +lcmap.j = round(w1); %relative to center of image +lcmap.i = round(w2); %relative to center of image + +%map image center relative indexing +%to standard matlab array indexing +w2 = f2dim/2+1-lcmap.i(:); +w1 = f1dim/2+1+lcmap.j(:); + +%vector of indexes to map cartesian to log-polar +lut.indvec = w2+(w1-1)*f2dim; + +[w2lcmap,w1lcmap] = size(lcmap.i); +lut.base = base; +lut.hscale = hscale; +lut.vscale = vscale; +lut.w1lcmap = w1lcmap; +lut.w2lcmap = w2lcmap; +lut.f1dim = f1dim; +lut.f2dim = f2dim; +lut.tdim = tdim; +lut.rdim = rdim; +lut.thresh = thresh; \ No newline at end of file diff --git a/src/utilities/uwit/fft_code/lutmapper.m b/src/utilities/uwit/fft_code/lutmapper.m new file mode 100644 index 0000000000000000000000000000000000000000..f88820f7392a090e3f0e4be81c23ed1ae3b34722 --- /dev/null +++ b/src/utilities/uwit/fft_code/lutmapper.m @@ -0,0 +1,11 @@ +function N = lutmapper(M,ylcmap,xlcmap,indvec) +% maps cartesian spectrum to log-polar VERY VERY quickly +% OP 22-APR-2000 +% ylcamp,xlcmap are dimension of the mapped image +% in log-polar coordinates +% indvec contains the coordinates of the cartesian representation +% ordered so they map into N sequentially +N = zeros(ylcmap,xlcmap); + +T = M(:); +N(:) = T(indvec); \ No newline at end of file diff --git a/src/utilities/uwit/fft_code/resolve_xlate2.m b/src/utilities/uwit/fft_code/resolve_xlate2.m new file mode 100644 index 0000000000000000000000000000000000000000..296180ee90f8cfd32f165fa2ccc9f2b23d689b08 --- /dev/null +++ b/src/utilities/uwit/fft_code/resolve_xlate2.m @@ -0,0 +1,100 @@ + + +%Resolve the periodicity of translation by a hypoyhesize-test scheme +%Dec.08.2002 Ali Can + +function [tu, tv] = resolve_xlate2(I1, I2, scale, theta, t, cntr, varargin) + + +%=============================================== +% CHECK FOR OPTIONAL ARGUMENTS for the mask +%=============================================== +NARGIN = nargin; +Imaskflag = 0; +if NARGIN < 6 + error('Invalid number of arguments'); +elseif NARGIN > 6 + done = 0; + ii = 1; + while ~done + switch lower(varargin{ii}) + case 'mask'; Imask = varargin{ii+1}; ii=ii+2; Imaskflag = 1; + otherwise + msg = sprintf('Unknown argument given'); + error(msg); + end + if ii >= length(varargin); done = 1; end + end +end + +if Imaskflag == 1 + I1 = immultiply(imadd(I1,1),uint8(Imask)); + I2 = immultiply(imadd(I2,1),uint8(Imask)); + thresh = 0.1 * sum(Imask(:)); +else + thresh = 0.1 * size(I1,1) *size(I1,2); +end; + +Cx(1) = t(1); +Cy(1) = t(2); + +%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% +%Minimum overlap is assumed to be 10 percent +if t(1) < -0.1 * size(I1,2) + Cx(2) = t(1) + size(I1,2); +elseif t(1) > 0.1 * size(I1,2) + Cx(2) = t(1) - size(I1,2); +end; +if t(2) < -0.1 * size(I1,1) + Cy(2) = t(2) + size(I1,1); +elseif t(2) > 0.1 * size(I1,1) + Cy(2) = t(2) - size(I1,1); +end; +Cxx = Cx +Cyy = Cy + +min_sum = 255; +tu = Cx(1); +tv = Cy(1); +%test the translation hypothesis +for ii=1:length(Cx) + for jj=1:length(Cy) + tform = tform_from_similarity(scale,theta,[Cx(ii), Cy(jj)],cntr); + + + %warp the input image I2 into I1's frame using + %the determined transform. calculate the warped + %image bounds so that both images are completly + %contained + inbounds = [1 1; size(I1,2) size(I1,1)]; + outbounds = findbounds(tform,inbounds); + mosaicbounds(1,:) = min(inbounds(1,:),outbounds(1,:)); + mosaicbounds(2,:) = max(inbounds(2,:),outbounds(2,:)); + xdata = mosaicbounds(:,1)'; + ydata = mosaicbounds(:,2)'; + J2 = imtransform(I2,tform,'bicubic','xdata',xdata,'ydata',ydata); + + %place the original I1 image into the mosaic canvas + J1 = imtransform(I1,maketform('affine',eye(3)),'bicubic', ... + 'xdata',xdata,'ydata',ydata); + + %find the pixels in the overlapping area + ind = find((J1~=0) & (J2~=0)); + JT = double(J1(ind)); + JN1 = (JT - mean(JT)) / (std(JT)+0.000001); + JT = double(J2(ind)); + JN2 = (JT - mean(JT)) / (std(JT)+0.000001); + Mdiff = sum(abs(JN1 - JN2)); + Mdiff = Mdiff / (length(ind)+1) + + LL = length(ind) + %impose at least 10 percent overlap + if ((length(ind) > thresh) & (Mdiff < min_sum)) + min_sum = Mdiff; + tu = Cx(ii); + tv = Cy(jj); + end; + clear ind; + end; +end; + diff --git a/src/utilities/uwit/fft_code/rstreg.m b/src/utilities/uwit/fft_code/rstreg.m new file mode 100644 index 0000000000000000000000000000000000000000..ee414d9ebcc0e2d32f738e76073bb4fbc0c825fe --- /dev/null +++ b/src/utilities/uwit/fft_code/rstreg.m @@ -0,0 +1,484 @@ +function [varargout] = rstreg(I1,I2,varargin) +%RSTREG Registers an image pair for rotation, scale, and +% and tranlation using Fourier transform properties. +% +% [SCALE,THETA,TRAN] = RSTREG(I1,I2) registers a pair of images +% I1,I2 using Fourier based methods which can account for images +% which are related through a scaling, rotation, and translation. +% The parameters SCALE,THETA, and TRAN are all measured relative +% to I1's coordinate frame. TRAN is a 2x1 vector of translations +% [tu tv]'. +% +% [SCALE,THETA,TRAN,J] = RSTREG(I1,I2) and +% [J] = RSTREG{I1,I2) optionally return the registered +% output image I2 in I1's coordinate frame. +% +% RSTREG(I1,I2) without any output argument assignments generates +% output figures only. +% +% The I1,I2 pair can be followed by parameter/value pairs to +% specify additional arguments. The following parameter/value +% arguments may be specified (not case sensitive): +% Paramter/Value Comment +% ------------------------------------------------------- +% 'F1DIM' # of samples of FFT in column direction +% 'F2DIM' # of samples of FFT in row direction +% 'RDIM' # of samples in log-polar radial coordinate +% 'TDIM' # of samples in log-polar theta coordinate +% 'CWIN' size of cosine window support +% 'THRESH' controls oversampling of pixels with small +% radial component, default 600 +% 'SHOW_FIGS' 0 produces no figures +% 1 produces only registered output image figures +% 2 produces lots of figures +% +%EXAMPLE1 +% +% I1 = double(rgb2gray(imread('westconcordaerial.png'))); +% [rdim,cdim] = size(I1); +% I2 = imresize(I1,1.2,'bicubic'); +% I2 = imrotate(I2,25.0,'bicubic','crop'); +% I2 = I2(1:rdim,1:cdim); +% rstreg(I1,I2); +% +%EXAMPLE2 +% +% I1 = double(imread('images/ESC.0565.tif')); +% I2 = double(imread('images/ESC.0564.tif')); +% rstreg(I1,I2); + +%HISTORY +%DATE WHO COMMENT +%-------- ------------- ----------------------------- +%05/2002 Ryan Eustice Streamlined and speed up original code. +%05/2000 Oscar Pizarro Originally created and implemented +% all functionality of algorithm. + + +show_figs = 0; +%=============================================== +% CHECK FOR OPTIONAL ARGUMENTS +%=============================================== +NARGIN = nargin; +if NARGIN < 2 + error('Invalid number of arguments'); +elseif NARGIN > 2 + done = 0; + ii = 1; + while ~done + switch lower(varargin{ii}) + case 'f1dim'; f1dim = varargin{ii+1}; ii=ii+2; + case 'f2dim'; f2dim = varargin{ii+1}; ii=ii+2; + case 'rdim'; rdim = varargin{ii+1}; ii=ii+2; + case 'tdim'; tdim = varargin{ii+1}; ii=ii+2; + case 'cwin'; cwin = varargin{ii+1}; ii=ii+2; + case 'thresh'; thresh = varargin{ii+1}; ii=ii+2; + case 'show_figs'; show_figs = varargin{ii+1}; ii=ii+2; + otherwise + msg = sprintf('Unknown argument given'); + error(msg); + end + if ii >= length(varargin); done = 1; end + end +end + +NARGOUT = nargout; +if NARGOUT == 0; + show_figs = 2; +end + + +%==================================== +% CREATE COSINE WINDOW THAT BRINGS +% INTENSITIES TO ZERO ON IMAGE BORDERS +%==================================== +[rowdim,coldim] = size(I1); +if ~exist('cwin','var') + %set default size of support + cos_support = 80; +end +cwin = coswin(rowdim,coldim,cos_support); + + +%============================== +% I1 CONTROL IMAGE +%============================== +if show_figs >= 1 + figure(1) + imagesc(I1); colormap gray; axis image; grid on; truesize; + title('Control image'); +end + +I1prime = double(I1); +I1prime = I1prime - mean(I1prime(:)); %remove DC mean intensity +I1prime = I1prime.*cwin; %smooth freq at border of image +if show_figs >= 2 + figure(2) + imagesc(I1prime); colormap gray; axis image; grid on; truesize; + title('Windowed Control image'); +end + +%=============================== +% I2 INPUT IMAGE +%=============================== +if show_figs >= 1 + figure(3) + imagesc(I2); colormap gray; axis image; grid on; truesize; + title('Input image'); +end + +I2prime = double(I2); +I2prime = I2prime - mean(I2prime(:)); %remove DC mean intensity +I2prime = I2prime.*cwin; %smooth freq at border of image +if show_figs >= 2 + figure(4) + imagesc(I2prime); colormap gray; axis image; grid on; truesize; + title('Windowed Input image'); +end + + +%================================== +% HIGH FREQ EMPHASIS FILTER +%================================== +fsize = 41; %assume odd filter support +if ~exist('sigma','var') + %set default + sigma = 1.5; %std dev in pixels +end + +%design filter's spatial impulse response +h = fspecial('log',fsize,sigma); + + +%==================================== +% APPLY FILTER TO IMAGES +%==================================== +%(f1dim,f2dim) are dimensions of FFT's +%which control zero padding and aliasing +%set defaults to be of equal dimension +if ~exist('f1dim') + f1dim = max(rowdim,coldim); %f1dim >= coldim +end +if ~exist('f2dim') + f2dim = max(rowdim,coldim); %f2dim >= rowdim +end + +%phase correction to translate filter to the origin +k1 = [0:f2dim-1]'; +k2 = [0:f1dim-1]; +phasecorrection = ... + (exp(j*k1*2*pi/(f2dim)*(fsize-1)/2)*exp(j*k2*2*pi/f1dim*(fsize-1)/2)); + +%apply laplacian of gaussian filter to images +%in the frequency domain with the correct phase +hF1 = fft2(I1prime,f2dim,f1dim); +hF2 = fft2(I2prime,f2dim,f1dim); +fH = fft2(h,f2dim,f1dim); +hF1 = hF1.*fH.*phasecorrection; +hF2 = hF2.*fH.*phasecorrection; + + +%===================================== +% MAGNITUDE OF FT OF FILTERED IMAGES +%===================================== +M1 = (fftshift(abs(hF1))); +M2 = (fftshift(abs(hF2))); +if show_figs >= 2 + figure(5); + imagesc(M1); axis image; grid on; + title('Magnitude of spectrum. Filtered Control image.'); + crange = caxis; + + figure(6); + imagesc(M2,crange); axis image; grid on; + title('Magnitude of spectrum. Filtered Input image.'); +end + + +%======================================= +% INVERSE TRANSFORM SPECTRA TO GET +% FILTERED IMAGES IN SPATIAL DOMAIN +%======================================= +if NARGOUT == 0 | show_figs == 2 + hI1 = (real(ifft2(hF1))); + hI1 = hI1(1:rowdim,1:coldim); + hI2 = (real(ifft2(hF2))); + hI2 = hI2(1:rowdim,1:coldim); +end +if show_figs >= 2 + figure(7); + imagesc(hI1); axis image; grid on; truesize; + title('Filtered Control image.'); + crange = caxis; + + figure(8); + imagesc(hI2,crange); axis image; grid on; truesize; + title('Filtered Input image.'); +end + +%conserve memory and clear intermediate variables +clear I1prime I2prime H phasecorrection; + + +%============================================ +% CREATE LOG-POLAR CONVERSION LOOK-UP-TABLE +% IF IT DOESN'T ALREADY EXIST +%============================================ +tol = 0.001; +Hmin = 0.03; + +%find radius of bandpass annulus of +%highpass emphasis filter by looking in +%upper left quadrant of fft +%(fft calculated w/o correction for fftshift) +wdim1 = find(Hmin-tol < abs(fH(1,1:f1dim/2)) & ... + Hmin+tol > abs(fH(1,1:f1dim/2))); +wdim2 = find(Hmin-tol < abs(fH(1:f2dim/2,1)) & ... + Hmin+tol > abs(fH(1:f2dim/2,1))); + +%keep the maximum radial dimension +wdim1 = max(wdim1); +wdim2 = max(wdim2); + +%choose the minimum of the two frequency axis +%components as the size of the frequency annulus +%to map into log-polar coordinates +wdim = min(wdim1,wdim2); + + +%(rdim,tdim) are dimensions of log-polar +%mapped image representing the number of +%samples in the (log(rho),theta) axes repectively +%set defaults to be <= min(f1dim,f2dim) +%frequency resolution in log-polar space +%cannot be any better than resolution in +%cartesian domain +if ~exist('rdim') + rdim = min(f1dim,f2dim); %# of samples in log(rho) axis +end +if ~exist('tdim') + tdim = min(f1dim,f2dim); %# of samples in theta axis +end + +%check to see if a previously cached LUT exists +%otherwise create it +lutfile = strcat(tempdir,'lut.mat'); +if exist(lutfile,'file') + load(lutfile); +end +createLUT = 0; +if exist('lut','var') + if lut.f1dim ~= f1dim | lut.f2dim ~= f2dim + createLUT = 1; + elseif lut.tdim ~= tdim | lut.rdim ~= rdim + createLUT = 1; + elseif exist('thresh','var'); + if lut.thresh ~= thresh + createLUT = 1; + end + end +else + createLUT = 1; +end +if createLUT + if exist('thresh','var'); + fprintf('Creating LUT with thresh = %g',thresh); + else + fprintf('Creating LUT ...'); + end + if exist('thresh','var') + lut = lplut(f1dim,f2dim,wdim,rdim,tdim,thresh); + else + lut = lplut(f1dim,f2dim,wdim,rdim,tdim); + end + save(strcat(tempdir,'lut.mat'),'lut'); + fprintf(' done\n\n'); +else + fprintf('Using cached LUT with thresh = %g ...\n\n',lut.thresh); +end + + +%========================================== +% MAP MAGNITUDE SPECTRUMS TO LOG-POLAR SPACE +%========================================== +N1 = (lutmapper(M1,lut.w2lcmap,lut.w1lcmap,lut.indvec)); +N2 = (lutmapper(M2,lut.w2lcmap,lut.w1lcmap,lut.indvec)); +[tdim,rdim] = size(N1); +if show_figs >= 2 + figure(9); + imagesc([],[0:tdim-1]/lut.vscale,N1); grid on; + title('Log-polar Magnitude of Spectrum. Control Image.'); + xlabel('samples of log(\rho), [\rho measured in pixels]'); + ylabel('\theta [deg]') + crange = caxis; + + figure(10); + imagesc([],[0:tdim-1]/lut.vscale,(N2),crange); grid on; + title('Log-polar Magnitude of Spectrum. Input Image.'); + xlabel('samples of log(\rho), [\rho measured in pixels]'); + ylabel('\theta [deg]') +end + + +%=========================================== +% CALCULATE SCALE AND THETA PARAMETERS VIA +% PHASE CORRELATION TECHNIQUE +% -->circular convolution in the theta direction +% -->linear convolution in the radius direction +%=========================================== +%Q1 & Q2 are the transforms of N1 & N2, +%the log-polar representations of M1 & M2 +Q1 = fft2(N1,tdim,2*rdim-1); +Q2 = fft2(N2,tdim,2*rdim-1); + +%conserve memory and clear intermediate variables +clear N1 N2 M1 M2; + +%calculate the cross-power spectrum +CPS = Q1.*conj(Q2); +CPS = CPS./abs(CPS); + +%inverse transform cross-power spectrum +%to get an impulse +peakM = real(fftshift(ifft2(CPS))); + +%conserve memory and clear intermediate variables +clear Q1 Q2 CPS; + +%find coordinates of impulse to recover +%scale and theta parameters +[yM,xM] = size(peakM); +maxval = max(peakM(:)); +[iind,jind] = find(peakM == maxval); + +% refer to image coordinates +jmax = jind - (xM/2) - 1; +imax = iind - (yM/2) - 1; + + +%============================================ +% CALCULATE SCLAE AND ROTATION USING +% MAPPING PARAMETERS +%============================================ +scale = lut.base^(jmax/lut.hscale); +theta = -imax/lut.vscale; %multiply by -1 to conform to + %convention of theta defined + %positive counter-clockwise + + +%============================================= +% DETERMINE TRANSLATIONAL OFFSET AND +% RESOLVE 180 DEGREE AMBIGUITY IN THETA +%============================================= +%transform filtered input image according to theta and scale +hJ2 = imrotate(hI2,-theta,'bilinear','crop'); +hJ2 = imresize(hJ2,1/scale,'bilinear'); +hJ2flip = flipud(fliplr(hJ2)); + +% find translation using phase correlation technique +[tu1,tv1,peakcorr1] = find_xlate2(hI1,hJ2); +[tu2,tv2,peakcorr2] = find_xlate2(hI1,hJ2flip); +if peakcorr1 > peakcorr2 + tu = tu1; + tv = tv1; + peakcorr = peakcorr1; +else + tu = tu2; + tv = tv2; + peakcorr = peakcorr2; + theta = theta+180; +end + +%conserve memory and clear intermediate variables +clear tu1 tv1 peakcorr1 tu2 tv2 peakcorr2 hI2 hJ2flip; + +%display results +fprintf('scale = %g, res = %g, rdim = %g samples ...\n', ... + scale,lut.base^(1/lut.hscale)-1,rdim); +fprintf('theta = %g degrees, res = %g, tdim = %g samples ...\n', ... + theta,1/lut.vscale,tdim); +fprintf('translation (x,y) = (%g,%g) ...\n',tu,tv); + + +%============================================= +% CREATE A MATLAB TFORM STRUCTURE WHICH +% CAN BE USED BY IMTRANSFORM TO WARP +% THE INPUT IMAGE +%============================================= +%calculate a pre-multiply homography H12 which +%rotates and scales I2 towards I1 +D2R = pi/180; +H12 = eye(3); +H12(1:2,1:2) = (1/scale)*[cos(theta*D2R) -sin(theta*D2R); + sin(theta*D2R) cos(theta*D2R)]; +H12(1:2,3) = -[tu; tv]; + +%MATLAB's tform structure assumes a post-multiply +%homography so use transpose of H12 +tform = maketform('affine',H12'); + +if show_figs >= 1 + figure(11); + %warp the input image I2 using the + %determined transform. + %note that MATLAB's imtransform function + %doesn't not place the image J2 in mosaic + %space using the translation offsets + %(tu,tv). this has to be done manually. + % J2rs = imtransform(I2,tform,'bicubic'); + J2rs = imrotate(I2,-theta,'bilinear','crop'); + J2rs = imresize(J2rs,1/scale,'bilinear'); + imagesc(J2rs); colormap gray; axis on; grid; truesize; + title('Rotated and Scaled Input Image'); +end + +if show_figs >= 1 + figure(12); + %place the rotated and scaled image in + %the control image's coordinate frame + [Jy Jx] = size(J2rs); + if tv < 0 + yi = 1-tv; + yf = min(rowdim,Jy-tv); + vi = 1; + vf = min(rowdim+tv,Jy); + else + yi = 1; + yf = min(rowdim,Jy-tv); + vi = tv+1; + vf = min(Jy,rowdim+tv); + end + + if tu < 0 + xi = 1-tu; + xf = min(coldim,Jx-tu); + ui = 1; + uf = min(coldim+tu,Jx); + else + xi = 1; + xf = min(coldim,Jx-tu); + ui = tu+1; + uf = min(Jx,coldim+tu); + end + + %windows so it will match frame of control image + J2 = zeros(rowdim,coldim); + J2(yi:yf,xi:xf) = J2rs(vi:vf,ui:uf); + imagesc(J2); colormap gray; axis on; grid; truesize; + title('Registered Input Image'); +end + + +%========================================== +% ASSIGN OUTPUT ARGUMENTS +%========================================== +if NARGOUT > 0 + if NARGOUT >= 3 + varargout{1} = scale; + varargout{2} = theta; + varargout{3} = [tu tv]; + end + if NARGOUT >= 4 + varargout{4} = J2; + end +end \ No newline at end of file diff --git a/src/utilities/uwit/fft_code/tform_from_similarity.m b/src/utilities/uwit/fft_code/tform_from_similarity.m new file mode 100644 index 0000000000000000000000000000000000000000..ea1e7bdf75636aa60ebbe5aba346ed09d5141983 --- /dev/null +++ b/src/utilities/uwit/fft_code/tform_from_similarity.m @@ -0,0 +1,28 @@ +function tform = tform_from_similarity(scale,theta,t,cntr) + +%============================================= +% CREATE A MATLAB TFORM STRUCTURE WHICH +% CAN BE USED BY IMTRANSFORM TO WARP +% THE INPUT IMAGE +%============================================= +%calculate a pre-multiply homography H12 which +%rotates and scales I2 towards I1 +D2R = pi/180; +H12 = eye(3); +R12 = [cos(theta*D2R) -sin(theta*D2R); + sin(theta*D2R) cos(theta*D2R)]; +H12(1:2,1:2) = (1/scale)*R12; +H12(1:2,3) = t'; + + +%create a homography to map a coordinate system +%located at the center of the image, to one which +%is located at the top left corner +xo = cntr(2); yo = cntr(1); +Htl = [1 0 xo; + 0 1 yo; + 0 0 1]; + +%MATLAB's tform structure assumes a post-multiply +%homography so use transpose of H12 +tform = maketform('affine',(Htl*H12*Htl^-1)'); \ No newline at end of file