diff --git a/calibration/automatic_filter.m b/calibration/automatic_filter.m
new file mode 100644
index 0000000000000000000000000000000000000000..1eebc873946aa1ce35c1006912e74e5c20574181
--- /dev/null
+++ b/calibration/automatic_filter.m
@@ -0,0 +1,63 @@
+cd%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
+% automatic_filter.m
+%
+% utility to help find the best filter size for identifying markers
+% for a given image file, the Laplacian of Gaussian method for finding
+% markers will be applied and the centre three markers extracted
+% the filter scale can be adjusted to allow experimentation
+
+addpath([pwd '/../../mex/minmax/']);
+addpath([pwd '/../']);
+addpath([pwd '/pinhole/']);
+addpath([pwd '/../gmv/']);
+
+%% input parameters
+
+grid_dx = 2.54; % real units, mm
+grid_dy = 2.54;
+filter_type = 'disk';
+
+input_file = '/nfs/scratch23/lawson/K62/170203/calib/POS01.cam1.tif';
+
+%% load image
+[~,~,ext] = fileparts(input_file);
+if strcmpi(ext, '.gmv')
+ % loading of .gmv movie files
+ im = mean(double(read_gmv_matlab(input_file)), 3);
+ im = fliplr(im);
+else
+ % loading of regular image files
+ im = double(imread(input_file));
+ im = fliplr(im');
+end
+Ni = size(im, 1);
+Nj = size(im, 2);
+i_axis = 0 : Ni-1;
+j_axis = 0 : Nj-1;
+
+%% loop
+while true
+ %% get filter scale
+ s_resp = input('Enter filter scale in pixels or q to quit: ', 's');
+ if strcmp(s_resp, 'q')
+ break;
+ end
+ blob_scale = str2double(s_resp);
+ if isnan(blob_scale)
+ continue;
+ end
+
+ %% filter
+ [ij_small,im_filt] = log_find_markers(i_axis, j_axis, im, blob_scale, 1000, filter_type);
+ [ij_big,im_filt_big] = log_find_markers(i_axis, j_axis, im, 2*blob_scale, 3, filter_type);
+
+ %% plot
+ figure(1);
+ clf;
+ imagesc(i_axis, j_axis, im_filt');
+ hold on;
+ plot(ij_small(1,:), ij_small(2,:), 'r.');
+ plot(ij_big(1,:), ij_big(2,:), 'ro');
+ axis equal tight xy;
+ colorbar;
+end
\ No newline at end of file
diff --git a/calibration/bounding_box.m b/calibration/bounding_box.m
new file mode 100644
index 0000000000000000000000000000000000000000..4dcd9cc2d4c26aacbfb4835fc1f3b3e983a04bd6
--- /dev/null
+++ b/calibration/bounding_box.m
@@ -0,0 +1,49 @@
+function [bb_lim, A, b] = bounding_box(camera)
+ % bbmax = bounding_box(calib)
+ % determine the corners of the smallest axis aligned bounding box
+ % which fit encapsulate the extent of the measurement volume
+ %
+ % bbmax is a 2 x 3 matrix containing the x, y and z limits of the
+ % smallest bounding box which encapsulate the entire measurement volume
+
+ %% get view frustums
+ % M = M_ary(:,p,c) are the coefficients for the equation of plane p of the
+ % viewing frustum for camera c.
+ % i.e.
+ % M(1)x + M(2)y + M(3)z + M(4) >= 0
+ % when a point lies within the measurement volume
+ n_cameras = length(camera);
+ M_ary = zeros(4,4,n_cameras);
+ for c = 1 : n_cameras
+ M_ary(:,:,c)= frustum(camera(c))';
+ end
+
+ %% solve linear programming problem
+ % to obtain volume bounds, we find the x that
+ % minimises c'x subject to Ax < b
+ A = reshape(-M_ary(1:3,:,:), [3 4*n_cameras])';
+ b = reshape( M_ary(4 ,:,:), [4*n_cameras 1]);
+ bb_lim = zeros(2,3);
+ opts = optimoptions('linprog', 'Display', 'off');
+ for i = 1 : 3
+ e_i = [0 0 0]';
+ e_i(i) = 1;
+ bb_lim(1,i) = dot(e_i, linprog(+e_i, A, b, [], [], [], [], [], opts));
+ bb_lim(2,i) = dot(e_i, linprog(-e_i, A, b, [], [], [], [], [], opts));
+ end
+
+ %% test
+ %{
+ N = 1E5;
+ bb_size = bb_lim(2,:) - bb_lim(1,:);
+ gl_pos = bsxfun(@plus, bb_lim(1,:)', diag(bb_size) * rand(3,N));
+ bb_dist = bsxfun(@minus, A*gl_pos, b);
+ b_inside = all(bb_dist <= 0, 1);
+ figure(1);
+ clf;
+ plot3(gl_pos(1, b_inside), gl_pos(2, b_inside), gl_pos(3, b_inside), 'g.');
+ hold on;
+ plot3(gl_pos(1,~b_inside), gl_pos(2,~b_inside), gl_pos(3,~b_inside), 'r.');
+ axis equal tight xy;
+ %}
+end
\ No newline at end of file
diff --git a/calibration/bounding_polygon.m b/calibration/bounding_polygon.m
new file mode 100644
index 0000000000000000000000000000000000000000..d3d96dc3ae0ab4dfd62133adfa461896f6586217
--- /dev/null
+++ b/calibration/bounding_polygon.m
@@ -0,0 +1,34 @@
+function [A,b] = bounding_polygon(corners)
+ % [A,b] = bounding_polygon(corners)
+ % determine the coefficients of an inequality Ay <= b such that
+ %
+ % if Ay <= b
+ % if polygon is convex:
+ % y is inside polygon
+ % else
+ % y is inside the convex hull of the polygon
+ % else
+ % y is outside polygon
+ %
+
+ %% get convex hull
+ idx = convhull(corners(1,:)', corners(2,:)');
+ corners = corners(:,idx);
+
+ %% get coefficients
+ n_vert = size(corners,2)-1;
+ A = zeros(n_vert,2);
+ b = zeros(n_vert,1);
+
+ for v = 1 : n_vert
+ % get normal of edge from y0 -> y1
+ y0 = corners(:, v);
+ y1 = corners(:, v+1);
+ y01 = y1 - y0;
+ n = [y01(2); -y01(1)];
+ % we are inside if (y - y0).n <= 0
+ % i.e. n.y <= n.y0
+ A(v,:)= n';
+ b(v) = dot(n, y0);
+ end
+end
\ No newline at end of file
diff --git a/calibration/bounding_polyhedron.m b/calibration/bounding_polyhedron.m
new file mode 100644
index 0000000000000000000000000000000000000000..33e548c4f008cf6baf7a4ed8c872ed5d68b2690d
--- /dev/null
+++ b/calibration/bounding_polyhedron.m
@@ -0,0 +1,42 @@
+function [A_hull,b_hull] = bounding_polyhedron(tri, vert)
+ % [A,b] = bounding_polyhedron(tri,vert)
+ %
+ % determine the system of linear constraints
+ % Ax <= b
+ % that describe the inside of the convex polyhedron described by
+ % tri and vert. That is, if x lies within the polyhedron, Ax<=b
+ %
+ % vert is a 3 x n_vert array of vertex coordinates
+ % tri is a n_face x 3 array of indices, describing the triangular
+ % faces of the polyhedron
+
+ n_vert = size(vert, 2);
+ n_face = size(tri, 1);
+
+ %% determine surface normals
+ % vertices of each triangular face
+ A = vert(:, tri(:,1));
+ B = vert(:, tri(:,2));
+ C = vert(:, tri(:,3));
+ D = mean(vert, 2);
+ DA = bsxfun(@minus, A, D);
+ % surface normal
+ n = cross(B-A, C-A, 1);
+ n = bsxfun(@rdivide, n, sqrt(sum(n.^2,1)));
+ % sign of surface normal is outwards positive
+ % D is inside polyhedron so (A-D).n >= 0
+ S = sign(sum(DA.*n, 1));
+ n = bsxfun(@times, S, n);
+
+ %% equation of plane: x.n + d = 0
+ d = -sum(n.*A, 1);
+
+ %% determine linear constraints
+ % each equation of plane has been chosen so that
+ % n.x + d = 0
+ % and if inside
+ % n.x + d > 0
+ % since n points outwards
+ A_hull = n';
+ b_hull = -d';
+end
\ No newline at end of file
diff --git a/calibration/bulk_marker_search.m b/calibration/bulk_marker_search.m
new file mode 100644
index 0000000000000000000000000000000000000000..89b1d376cfb121abd447480f7d722852e969b348
--- /dev/null
+++ b/calibration/bulk_marker_search.m
@@ -0,0 +1,276 @@
+%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
+% bulk_marker_search.m
+% find markers in images of calibration plates
+% then perform a 2D calibration to dewarp the calibration plate image
+
+addpath([pwd '/../mex/minmax/']);
+
+%% input parameters
+
+grid_dx = 10; % real units, mm
+grid_dy = 10;
+invert_image = 1; % invert image for black dots on white
+blur_image = 1; % gaussian blur image
+normalise_image = 1;
+xlim_zoom = 1024*2/4 + [-512 512]/1;
+ylim_zoom = 1024*2/4 + [-512 512]/1;
+
+output_dir = 'D:/bigtank-jul-2013/CAL-20-07-13/NEWSHEET/';
+input_file_list = { 'D:/bigtank-jul-2013/CAL-20-07-13/SHEET-01/PLATE/CAM1_000001.tif', ...
+ 'D:/bigtank-jul-2013/CAL-20-07-13/SHEET-02/PLATE/CAM1_000001.tif', ...
+ 'D:/bigtank-jul-2013/CAL-20-07-13/SHEET-03/PLATE/CAM1_000001.tif', ...
+ 'D:/bigtank-jul-2013/CAL-20-07-13/SHEET-04/PLATE/CAM1_000001.tif', ...
+ 'D:/bigtank-jul-2013/CAL-20-07-13/SHEET-05/PLATE/CAM1_000001.tif', ...
+ 'D:/bigtank-jul-2013/CAL-20-07-13/SHEET-06/PLATE/CAM1_000001.tif', ...
+ 'D:/bigtank-jul-2013/CAL-20-07-13/SHEET-07/PLATE/CAM1_000001.tif', ...
+ 'D:/bigtank-jul-2013/CAL-20-07-13/SHEET-08/PLATE/CAM1_000001.tif', ...
+ 'D:/bigtank-jul-2013/CAL-20-07-13/SHEET-09/PLATE/CAM1_000001.tif', ...
+ 'D:/bigtank-jul-2013/CAL-20-07-13/SHEET-10/PLATE/CAM1_000001.tif'};
+plate_origin = [-24 0
+ -20 0
+ -16 0
+ -12 0
+ -8 0
+ -4 0
+ 0 0
+ 4 0
+ 8 0
+ 12 0]';
+camera_index = 1;
+dot_half_window = [24 24];
+calib_idx_offset = 0;
+origin_pos = [0 0];
+
+camera_clim = [0 1];
+
+%% generate calibration for each image
+
+nCalibrations = length(input_file_list);
+
+calib_idx = 1;
+
+while calib_idx <= nCalibrations
+ %% update user of status
+ input_file = input_file_list{calib_idx};
+ output_file = sprintf('%s/CAM%d-%02d.mat', output_dir, camera_index, calib_idx+calib_idx_offset);
+ fprintf('Image %02d of %02d:\n', calib_idx, nCalibrations);
+
+ % check if calibration already exists
+ if exist(output_file, 'file')
+ sInput = input('Calibration already exists - overwrite? y to overwrite: ', 's');
+ if ~strcmpi(sInput, 'y')
+ calib_idx = calib_idx + 1;
+ continue;
+ end
+ end
+ % check if image input exists
+ if ~exist(input_file, 'file')
+ fprintf(' - could not find image file\n');
+ end
+
+ %% load image, request position
+ im = imread(input_file)';
+ im = double(im);
+ im = fliplr(im);
+ Nx = size(im, 1);
+ Ny = size(im, 2);
+
+ %% create filtered image for processing
+ im_filt = im;
+ if invert_image
+ im_filt = max(im(:))-im;
+ end
+ if blur_image
+ G = fspecial('gaussian', [5 5]);
+ im_filt = filter2(G, im_filt);
+ end
+ if normalise_image
+ [minv, maxv] = minmaxfiltnd(single(im_filt), dot_half_window*2+1);
+ im_filt = (im_filt - minv) ./ max(32, maxv-minv);
+ end
+
+ %% display image
+ figure(gcf);
+ hold off;
+ imagesc(im_filt');
+ axis equal tight xy;
+ colormap gray;
+ set(gca,'Clim', camera_clim);
+
+ %% get center, right and top marker position
+ bDone = false;
+ while ~bDone
+ %% get marker position from user
+ fprintf('Please identify the origin, right and top markers:\n');
+ xlim(xlim_zoom);
+ ylim(ylim_zoom);
+ [i_coord,j_coord] = ginput(3);
+ axis tight;
+
+ %% find barycenter of centre marker
+ dot_img_i = round(i_coord(1)) + (-dot_half_window(1) : dot_half_window(1));
+ dot_img_j = round(j_coord(1)) + (-dot_half_window(2) : dot_half_window(2));
+ [imat,jmat] = ndgrid(dot_img_i, dot_img_j);
+ dot_img = im_filt(dot_img_i, dot_img_j);
+ ij_center(1) = sum(dot_img(:) .* imat(:)) / sum(dot_img(:));
+ ij_center(2) = sum(dot_img(:) .* jmat(:)) / sum(dot_img(:));
+
+ ij_right = [i_coord(2) j_coord(2)];
+ ij_top = [i_coord(3) j_coord(3)];
+
+ %% ask user if they want to keep it
+ hold on;
+ plot_markers = plot( ij_center(1), ij_center(2), 'bs', ...
+ ij_right(1), ij_right(2), 'ro', ...
+ ij_top(1), ij_top(2), 'gd');
+ %legend('Origin', 'Right', 'Top', 'Location', 'SouthOutside');
+
+ sDone = input('Enter y if you would like to try again: ', 's');
+ bDone = true;
+ if strcmpi(sDone,'y')
+ bDone = false;
+ end
+ delete(plot_markers);
+ end
+
+ %% find marker positions
+ [ij_markers, xy_markers, onx_idx, ony_idx] = find_calibration_markers(im_filt, ij_center, ij_right, ij_top, dot_half_window);
+ on_x_axis = false(1,size(ij_markers,2));
+ on_y_axis = false(1,size(ij_markers,2));
+ on_x_axis(onx_idx) = true;
+ on_y_axis(ony_idx) = true;
+ xy_markers(1,:) = xy_markers(1,:) * grid_dx + origin_pos(1);
+ xy_markers(2,:) = xy_markers(2,:) * grid_dy + origin_pos(2);
+
+ %% find positions more accurately using barycentres
+ Nmarkers = size(ij_markers,2);
+ for marker_idx = 1 : Nmarkers
+ dot_img_i = round(ij_markers(1,marker_idx)) + (-dot_half_window(1) : dot_half_window(1));
+ dot_img_j = round(ij_markers(2,marker_idx)) + (-dot_half_window(2) : dot_half_window(2));
+ dot_img_i = dot_img_i(dot_img_i > 1 & dot_img_i < Nx);
+ dot_img_j = dot_img_j(dot_img_j > 1 & dot_img_j < Ny);
+ dot_img = im_filt(dot_img_i, dot_img_j);
+ [imat,jmat] = ndgrid(dot_img_i, dot_img_j);
+ ij_markers(1,marker_idx) = sum(dot_img(:).*imat(:)) / sum(dot_img(:));
+ ij_markers(2,marker_idx) = sum(dot_img(:).*jmat(:)) / sum(dot_img(:));
+ end
+
+ %% throw away any markers too near edges
+ bad_markers = ij_markers(1,:) < 10 | ij_markers(1,:) > (Nx-10) ...
+ | ij_markers(2,:) < 10 | ij_markers(2,:) > (Ny-10);
+ xy_markers = xy_markers(:, ~bad_markers);
+ ij_markers = ij_markers(:, ~bad_markers);
+ on_x_axis = on_x_axis(~bad_markers);
+ on_y_axis = on_y_axis(~bad_markers);
+
+ %% plot x/y coords of each marker
+ n_markers = size(ij_markers, 2);
+
+ for idx = 1 : n_markers
+ str = sprintf('(%0.0f,%0.0f)', xy_markers(1,idx), xy_markers(2,idx));
+ text( 'Position', ij_markers(:,idx) + [-20; -20], ...
+ 'String', str, ...
+ 'FontSize', 8, ...
+ 'Color', 'red');
+ end
+
+ %% query user for markers to delete
+ bDone = false;
+ while ~bDone
+ figure(gcf);
+ plot_markers = plot( ij_markers(1,:), ij_markers(2,:), 'r+', ...
+ ij_markers(1,on_x_axis), ij_markers(2,on_x_axis), 'go', ...
+ ij_markers(1,on_y_axis), ij_markers(2,on_y_axis), 'gd', ...
+ ij_center(1), ij_center(2), 'bs');
+ %legend('Markers', 'X axis', 'Y axis', 'Center');
+
+ bDone = true;
+ sDelete = input('Would you like to delete a marker? y to delete: ', 's');
+ if strcmp(sDelete, 'y')
+ bDone = false;
+ fprintf('Click near the marker you''d like to delete\n');
+ [i_delete,j_delete] = ginput(1);
+ [idx, distance] = knnsearch(ij_markers', [i_delete,j_delete], 'K', 1);
+ if distance > 10
+ fprintf('You didn''t click anywhere near a marker!\n');
+ continue;
+ end
+ keep = (1 : size(ij_markers,2)) ~= idx;
+ ij_markers = ij_markers(:, keep);
+ xy_markers = xy_markers(:, keep);
+ on_x_axis = on_x_axis(keep);
+ on_y_axis = on_y_axis(keep);
+
+ delete(plot_markers);
+ end
+ end
+
+ %% create calibration and dewarp
+ xy_to_i_coeffs = cubicfit_2d(xy_markers(1,:), xy_markers(2,:), ij_markers(1,:), 1);
+ xy_to_j_coeffs = cubicfit_2d(xy_markers(1,:), xy_markers(2,:), ij_markers(2,:), 1);
+ ij_to_x_coeffs = cubicfit_2d(ij_markers(1,:), ij_markers(2,:), xy_markers(1,:), 1);
+ ij_to_y_coeffs = cubicfit_2d(ij_markers(1,:), ij_markers(2,:), xy_markers(2,:), 1);
+
+ i_fit = cubicmap_2d(xy_markers(1,:), xy_markers(2,:), xy_to_i_coeffs);
+ j_fit = cubicmap_2d(xy_markers(1,:), xy_markers(2,:), xy_to_j_coeffs);
+ i_error = ij_markers(1,:) - i_fit;
+ j_error = ij_markers(2,:) - j_fit;
+ px_error = sqrt(i_error.^2 + j_error.^2);
+ rms_error = sqrt(mean(px_error.^2));
+
+ fprintf('RMS fit error: %0.2f px\n', rms_error);
+
+ % work out x and y axes
+ [imat,jmat] = ndgrid(1 : 32 : Nx, 1 : 32 : Ny);
+ xmat = cubicmap_2d(imat, jmat, ij_to_x_coeffs);
+ ymat = cubicmap_2d(imat, jmat, ij_to_y_coeffs);
+ x_lim = fix([min(xmat(:)), max(xmat(:))]);
+ y_lim = fix([min(ymat(:)), max(ymat(:))]);
+ x_axis = linspace(x_lim(1), x_lim(2), Nx);
+ y_axis = linspace(y_lim(1), y_lim(2), Ny);
+ i_axis = 1 : Nx;
+ j_axis = 1 : Ny;
+ % calculate dewarping
+ [xmat,ymat] = ndgrid(x_axis, y_axis);
+ imat = cubicmap_2d(xmat, ymat, xy_to_i_coeffs);
+ jmat = cubicmap_2d(xmat, ymat, xy_to_j_coeffs);
+ im_xy = interp2(i_axis, j_axis, im', imat, jmat, 'linear');
+ hold off;
+ imagesc(x_axis, y_axis, im_xy');
+ xlim(x_lim);
+ ylim(y_lim);
+ colormap gray;
+ axis xy equal tight;
+ hold on;
+ set(gca,'Clim', camera_clim);
+ for x = (round(x_lim(1)/grid_dx) : round(x_lim(end)/grid_dx)) * grid_dx
+ plot([x x], [y_axis(1) y_axis(end)], 'r-');
+ end
+ for y = (round(y_lim(1)/grid_dy) : round(y_lim(end)/grid_dy)) * grid_dy
+ plot([x_axis(1) x_axis(end)], [y y], 'r-');
+ end
+
+ %% query user to save calibration or repeat
+
+ sInput = input('Would you like to repeat the calibration? y to repeat: ', 's');
+ if strcmpi(sInput, 'y')
+ continue; % continue without incrementing the counter
+ end
+
+ %% save output
+ fprintf('Saving ... ');
+ fstruct = struct('source_image', im, ...
+ 'dewarped_image', im_xy, ...
+ 'plate_pos', plate_origin(:, calib_idx), ...
+ 'xy_to_i_coeffs', xy_to_i_coeffs, 'xy_to_j_coeffs', xy_to_j_coeffs, ...
+ 'ij_to_x_coeffs', ij_to_x_coeffs, 'ij_to_y_coeffs', ij_to_y_coeffs, ...
+ 'i_axis', i_axis, 'j_axis', j_axis, ...
+ 'x_axis', x_axis, 'y_axis', y_axis, 'x_lim', x_lim, 'y_lim', y_lim, ...
+ 'grid_dx', grid_dx, 'grid_dy', grid_dy, ...
+ 'ij_markers', ij_markers, 'xy_markers', xy_markers, ...
+ 'ij_center', ij_center, 'ij_right', ij_right, 'ij_top', ij_top, ...
+ 'camera_index', camera_index);
+ save(output_file, '-struct', 'fstruct');
+ fprintf('done\n');
+ calib_idx = calib_idx + 1;
+end
\ No newline at end of file
diff --git a/calibration/bundle_calibration_fit.m b/calibration/bundle_calibration_fit.m
new file mode 100644
index 0000000000000000000000000000000000000000..b04c8a315b1b4b33b31d7dad93ab65323eabb54d
--- /dev/null
+++ b/calibration/bundle_calibration_fit.m
@@ -0,0 +1,185 @@
+function new_setup = bundle_calibration_fit(setup)
+ % perform a bundle calibration for a set of cameras and scene
+ % information provided in setup
+
+ %% extract basic information
+ n_cameras = setup.n_cameras;
+ n_views = setup.n_views;
+
+ %% initialise setup with a regular pinhole fit
+ fprintf('initial pinhole fit ... ');
+ s_cam_calib = pinhole_camera_fit(setup);
+ for c = 1 : n_cameras
+ setup.camera(c).calib = s_cam_calib(c);
+ end
+ fprintf('done\n');
+
+ %% initialise perturbation model
+ setup.omega = eye(3);
+ setup.gl_plate_dx = zeros(3, n_views);
+
+ %% fill arrays for least squares fitting
+ % uv_data should be n_total x 4 array of [u, v, cam, view]
+ % ij_data should be n_total x 2 array of [i, j] position of markers
+ n_total = 0;
+ uv_data = [];
+ ij_data = [];
+ for c = 1 : n_cameras
+ camera = setup.camera(c);
+ for v = 1 : n_views
+ %% extract marker position in plate coord sys and image
+ % xdata contains (u,v) coord of marker on plate
+ % as well as the index of the plate position and camera
+ % that the projection of this marker in image space corresponds to
+ uv_mark = camera.view(v).uv_markers;
+ ij_mark = camera.view(v).ij_markers;
+ n_mark = size(uv_mark,2);
+
+ uv_mark = [uv_mark; c*ones(1,n_mark); v*ones(1,n_mark)];
+ uv_data = [uv_data; uv_mark'];
+ ij_data = [ij_data; ij_mark'];
+ n_total = n_total + n_mark;
+ end
+ end
+
+ %% least squares fitting operation
+ fprintf('doing bundle fit ...');
+ X0 = pack_model(setup);
+ NX = length(X0);
+ opts = optimset('MaxFunEvals', NX*10000, 'MaxIter', 10000, 'Display', 'on');
+ X = lsqcurvefit(@(X,xdata) bundle_model(X,xdata,setup), X0, uv_data, ij_data, [], [], opts);
+ new_setup = unpack_model(X, setup);
+ fprintf('done\n');
+end
+
+function ij_selected = bundle_model(perturb_vec, uv_mark, setup)
+ % given a perturbation (packed into perturbation vector)
+ % project markers with perturbed model
+ setup = unpack_model(perturb_vec, setup);
+
+ %% perturbed position and orientation of plate
+ gl_plate_pos = setup.gl_plate_pos + setup.gl_plate_dx;
+ pl_to_gl = setup.omega * setup.pl_to_gl;
+
+ %% project markers
+ n_cameras = setup.n_cameras;
+ n_markers = size(uv_mark, 1);
+ cam = uv_mark(:, 3);
+ view = uv_mark(:, 4);
+ % gl_mark and pl_mark are 3 x n_markers matrices
+ pl_mark = [uv_mark(:, 1:2), zeros(n_markers,1)]';
+ gl_mark = pl_to_gl*pl_mark + gl_plate_pos(:, view);
+
+ % project markers in *every* camera
+ ij_mark = zeros(2, n_markers, n_cameras);
+ for c = 1 : n_cameras
+ ij_mark(:,:,c) = pinhole_model(gl_mark, setup.camera(c).calib);
+ end
+
+ % return only the appropriate projections, according to uv_mark(:,3)
+ ij_selected = zeros(n_markers, 2);
+ ij_selected(:,1) = ij_mark(sub2ind([2 n_markers n_cameras], 1*ones(n_markers,1), (1:n_markers)', cam));
+ ij_selected(:,2) = ij_mark(sub2ind([2 n_markers n_cameras], 2*ones(n_markers,1), (1:n_markers)', cam));
+end
+
+function X = pack_model(setup)
+ % function to pack a representation of the internal
+ % camera and scene model into a vector for least squares optimisation
+ % routine
+
+ n_cameras = setup.n_cameras;
+ n_views = setup.n_views;
+
+ %% camera model
+ X_cam = zeros(15, n_cameras);
+ for c = 1 : n_cameras
+ calib = setup.camera(c).calib;
+ G = calib.G;
+ R = calib.R;
+ r_vec = vrrotmat2vec(R);
+ r_vec = r_vec(1:3) * r_vec(4);
+ xc = calib.xc;
+ kr = calib.kr;
+ ic = calib.ic;
+
+ i_axis = setup.camera(c).i_axis;
+ j_axis = setup.camera(c).j_axis;
+ diag_px = sqrt((i_axis(end)-i_axis(1))^2 + (j_axis(end)-j_axis(1))^2);
+
+ X_cam(1,c) = G(1,1);
+ X_cam(2,c) = G(1,2);
+ X_cam(3,c) = G(1,3) / diag_px;
+ X_cam(4,c) = G(2,2);
+ X_cam(5,c) = G(2,3) / diag_px;
+ X_cam(6:8,c) = r_vec(:);
+ X_cam(9:11,c) = xc(:);
+ % dewarping parameters are normalised to dimensionless quantities,
+ % scaled by sensor size in px
+ X_cam(12:13,c) = [kr(1)*diag_px^2; kr(2)*diag_px^4];
+ X_cam(14:15,c) = ic(:) / diag_px;
+ end
+
+ %% plate perturbation model
+ % plate perturbation consists of a displacement and rotation
+ X_plate = setup.gl_plate_dx([1 3], :);
+ omega_vec = vrrotmat2vec(setup.omega);
+ X_rot = omega_vec(1:3) * omega_vec(4);
+
+ X = [X_cam(:); X_plate(:); X_rot(:)]';
+end
+
+function setup = unpack_model(X, setup)
+ % extract the perturbation model specified in X
+ % and update setup structure
+ n_views = setup.n_views;
+ n_cameras = setup.n_cameras;
+
+ %% extract
+ off = 1;
+ X_cam = reshape(X(off : off-1 + n_cameras*15), [15 n_cameras]);
+ off = off + n_cameras*15;
+ X_plate = reshape(X(off : off-1 + n_views*2), [2 n_views]);
+ off = off + n_views*2;
+ X_rot = X(off : off + 2);
+
+ %% update camera parameters
+ for c = 1 : n_cameras
+ i_axis = setup.camera(c).i_axis;
+ j_axis = setup.camera(c).j_axis;
+ diag_px = sqrt((i_axis(end)-i_axis(1))^2 + (j_axis(end)-j_axis(1))^2);
+
+ % intrinsic parameters
+ fx = X_cam(1,c);
+ tau = X_cam(2,c);
+ ox = X_cam(3,c) * diag_px;
+ fy = X_cam(4,c);
+ oy = X_cam(5,c) * diag_px;
+ G = [fx tau ox;
+ 0 fy oy;
+ 0 0 1];
+ % rotation
+ r_vec = X_cam(6:8,c);
+ R = vrrotvec2mat([r_vec(:)/(eps+norm(r_vec)); norm(r_vec)]);
+
+ % position
+ xc = X_cam(9:11,c);
+
+ % dewarping parameters are normalised to dimensionless quantities,
+ % scaled by sensor size in px
+ kr = [X_cam(12,c)/diag_px^2; X_cam(13,c)/diag_px^4]; % undo normalisation
+ ic = X_cam(14:15,c) * diag_px;
+
+ % full projection model
+ P = G*R*[eye(3), -xc(:)];
+
+ calib = struct('P', P, 'G', G, 'R', R, 'xc', xc, 'kr', kr, 'ic', ic);
+ setup.camera(c).calib = calib;
+ end
+
+ %% update plate position parameters
+ omega_vec = [X_rot(:)/(eps+norm(X_rot)); norm(X_rot)];
+ setup.omega = vrrotvec2mat(omega_vec);
+ setup.gl_plate_dx = [X_plate(1,:); zeros(1,n_views); X_plate(2,:)];
+end
+
+
diff --git a/calibration/calcmask.m b/calibration/calcmask.m
new file mode 100644
index 0000000000000000000000000000000000000000..3f2f086863725126a7ccfd829f737427dcf7e03d
--- /dev/null
+++ b/calibration/calcmask.m
@@ -0,0 +1,107 @@
+function mask = calcmask(camset, c0)
+ % mask = calcmask(camset, c)
+ %
+ % for each pixel in camera c of camset, determine the number
+ % of other cameras its epipolar ray is visible in
+
+ %% get things
+ n_cameras= length(camset);
+ cam0 = camset(c0);
+ Ni = length(cam0.i_axis);
+ Nj = length(cam0.j_axis);
+ Npx = Ni*Nj;
+
+ %% dewarp
+ [im,jm] = ndgrid(cam0.i_axis, cam0.j_axis);
+ ui_pos = inverse_radial_distortion([im(:),jm(:)]', cam0.calib.ic, cam0.calib.kr);
+
+ %% get equation of ray
+ % we write the equation of ray as
+ % x = x0 + s*e_s
+ ytilde = [ui_pos; ones(1,Npx)];
+ GR = cam0.calib.G * cam0.calib.R;
+ e_ray = GR \ ytilde;
+ x0_ray = cam0.calib.xc;
+
+ %% determine mask
+ mask = zeros(Ni, Nj);
+ for c1 = 1 : n_cameras
+ %% skip if same camera
+ if c0 == c1
+ continue;
+ end
+
+ %% find linear inequality for bounds test
+ % the test for whether a point y is within the bounds of image space
+ % of this camera is described as an inequality Ay <= b. If satisfied,
+ % the point is within bounds, if not, it is outside bounds. we
+ % construct this inequality based on borders of image in undistorted
+ % image space
+ cam1 = camset(c1);
+ i_axis = cam1.i_axis(:);
+ j_axis = cam1.j_axis(:);
+ Ni1 = length(i_axis);
+ Nj1 = length(j_axis);
+ ir = [1 : floor(Ni1/4) : Ni1, Ni1];
+ jr = [1 : floor(Nj1/4) : Nj1, Nj1];
+ i_corners = [i_axis(ir([1 1 1 1 1]));
+ i_axis(ir );
+ i_axis(ir([5 5 5 5 5]));
+ i_axis(ir(end:-1:1 ))];
+ j_corners = [j_axis(jr );
+ j_axis(jr([5 5 5 5 5]));
+ j_axis(jr(end:-1:1 ));
+ j_axis(jr([1 1 1 1 1]))];
+ di_corners = [i_corners'; j_corners'];
+ ui_corners = inverse_radial_distortion(di_corners, cam1.calib.ic, cam1.calib.kr);
+ [A,b] = bounding_polygon(ui_corners);
+
+ %% get equation of ray in image coordinates
+ % we want an equation of the form
+ % y = y0 + s * n
+ % that describes the equation of each ray from camera 0 in the
+ % (undistorted) image space of camera 1
+
+ GR1 = cam1.calib.G * cam1.calib.R;
+ z0 = GR1(1:2,:)*(x0_ray - cam1.calib.xc);
+ zprime = GR1(1:2,:)*e_ray;
+ lambda0 = GR1( 3,:)*(x0_ray - cam1.calib.xc);
+ lprime = GR1( 3,:)*e_ray;
+
+ y0 = bsxfun(@rdivide, zprime, lprime);
+ e_n = bsxfun(@minus, z0, lambda0*y0);
+ e_n = bsxfun(@rdivide, e_n, sqrt(sum(e_n.^2,1))); % normalise
+
+ %% determine existence of solutions for inequality
+ % subsituting in y = y0 + s*e_n
+ % into our inequality Ay <= b
+ % yields the inequality
+ % L*s <= R
+ % where L = A*e_n and R = b - A*y0
+ % if this inequality has solutions for any s, then there is at least
+ % one point on the ray which is visible in camera 1
+
+ L = A*e_n;
+ R = bsxfun(@minus, b, A*y0);
+ gamma_lim = R ./ L;
+ is_ub = L >= 0;
+ is_lb = L < 0;
+
+ % find smallest upper bound
+ gamma_ub = gamma_lim;
+ gamma_ub(is_lb) = inf; % ignore all those lower bounds
+ gamma_ub = min(gamma_ub, [], 1);
+
+ % find largest lower bound
+ gamma_lb = gamma_lim;
+ gamma_lb(is_ub) = -inf; % ignore all the upper bounds
+ gamma_lb = max(gamma_lb, [], 1);
+
+ b_visible = gamma_lb <= gamma_ub;
+
+ %% add to sum
+ b_visible = reshape(b_visible, [Ni Nj]);
+ mask = mask + b_visible;
+ end
+end
+
\ No newline at end of file
diff --git a/calibration/calibration_to_config.m b/calibration/calibration_to_config.m
new file mode 100644
index 0000000000000000000000000000000000000000..307ea705e3dd2f1e9b1f114124f667513c2be463
--- /dev/null
+++ b/calibration/calibration_to_config.m
@@ -0,0 +1,135 @@
+%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
+% based on a camera calibration performed in MATLAB using John Lawson's
+% code, create a corresponding .cfg file for use with Haitao Xu's PTV code
+%
+
+addpath([pwd '/../calibration-mpids/']);
+
+%% input parameters
+calib_dir = '/data/cluster/160323/';
+calib_file_mat = [calib_dir '/camera-model-160323.mat'];
+calib_file_cfg = [calib_dir '/PTVSetup_160323.cfg'];
+points_file_pat = [calib_dir '/calibpointspos.cam%d.dat'];
+
+pixel_pitch_mm = [10E-3 10E-3];
+
+%% load calibration
+fs = importdata(calib_file_mat);
+n_cameras = fs.n_cameras;
+camera = fs.camera;
+
+%% calculate Tsai camera calibration
+fprintf('Calculating Tsai calibration ... ');
+
+for c = 1 : n_cameras
+ %% load parameters
+ i_axis = camera(c).i_axis;
+ j_axis = camera(c).j_axis;
+ Ni = length(i_axis);
+ Nj = length(j_axis);
+ cparams = struct('Npixh', Nj, 'Npixw', Ni, ...
+ 'hpix', pixel_pitch_mm(1), ...
+ 'wpix', pixel_pitch_mm(2));
+
+ %% load markers and calculate residuals
+ gl_markers = camera(c).calib.gl_markers;
+ ij_markers = camera(c).calib.ij_markers;
+ ij_markers_proj = pinhole_model(gl_markers, camera(c).calib, true);
+ model_res = ij_markers - ij_markers_proj;
+
+ %% generate Tsai calibration
+ % Tsai calibration uses a different coordinate system for image plane
+ % in our calibration, (i,j) corresponds to x,y coordinate, as measured
+ % from the bottom left corner. ij the Tsai calibration, (i,j)
+ % coordinates start from the top left corner and move down and across.
+ ij_markers_perm = [ij_markers(1,:);
+ Nj-1 - ij_markers(2,:)];
+
+ [tsai_calib, tsai_res] = calib_Tsai(ij_markers_perm', gl_markers', cparams);
+ tsai_res = tsai_res';
+ camera(c).tsai_calib = tsai_calib;
+
+ %% save calibpointspos file
+ points_file = sprintf(points_file_pat, c - 1);
+ fid = fopen(points_file, 'w');
+ fprintf(fid, '# pimg_x (pix)\t pimg_y (pix) \t x (mm) \t y (mm) \t z (mm) \t Np \t sum(x) \t sum(y) \t sum(I) \n');
+ fprintf(fid, '%12.7f\t%12.7f\t%12.7f\t%12.7f\t%12.7f\t0\n', [ij_markers_perm; gl_markers]);
+ fclose(fid);
+
+ %% generate statistics of error
+ model_rms = rms(model_res, 2);
+ tsai_rms = rms(tsai_res, 2);
+
+ %% report on error
+ fprintf('Camera %d\n', c);
+ fprintf(' Regular: %0.3f %0.3f\n', model_rms);
+ fprintf(' Tsai: %0.3f %0.3f\n', tsai_rms);
+end
+fprintf('done\n');
+
+%% write config file
+fprintf('Saving to file ... ');
+
+% Now write the configuration file
+fid = fopen(calib_file_cfg, 'w');
+fprintf(fid, '# PTV experiment configuration file\n');
+fprintf(fid, '\n %d\t# ncams\n\n', n_cameras);
+for c = 1 : n_cameras
+ %% load info
+ tsai_calib = camera(c).tsai_calib;
+ i_axis = camera(c).i_axis;
+ j_axis = camera(c).j_axis;
+ Ni = length(i_axis);
+ Nj = length(j_axis);
+
+ %% write to file
+ fprintf(fid, '######## cam %d ########\n\n', c - 1);
+ fprintf(fid, '%d\t\t\t# Npix_x\n', Ni);
+ fprintf(fid, '%d\t\t\t# Npix_y\n', Nj);
+ fprintf(fid, '%11.8f\t\t\t# pixsize_x (mm)\n', pixel_pitch_mm(1));
+ fprintf(fid, '%11.8f\t\t# pixsize_y (mm)\n', pixel_pitch_mm(1));
+ fprintf(fid, '%12.8f\t\t# effective focal length (mm)\n', tsai_calib.f_eff);
+ % Note the sign change for k1, because its meaning is different in calib_Tsai and the stereomatching code
+ fprintf(fid, '%15.8e\t# radial distortion k1 (1/pixel)\n', -tsai_calib.k1);
+ fprintf(fid, '%15.8e\t# tangential distortion p1 (1/pixel)\n', tsai_calib.p1);
+ fprintf(fid, '%15.8e\t# tangential distortion p2 (1/pixel)\n', tsai_calib.p2);
+ fprintf(fid, '0.0\t\t# x0\n');
+ fprintf(fid, '0.0\t\t# y0\n');
+ % Parameters for camera movement correction
+ fprintf(fid, '0.0\t\t# x0_offset (pixel)\n');
+ fprintf(fid, '0.0\t\t# y0_offset (pixel)\n');
+ fprintf(fid, '1.0\t\t# x_rot (cos(theta))\n');
+ fprintf(fid, '0.0\t\t# y_rot (sin(theta))\n');
+ fprintf(fid, '# rotation matrix R\n');
+ fprintf(fid, '%12.8f\t%12.8f\t%12.8f\n', (tsai_calib.R)');
+ fprintf(fid, '# translation vector T\n');
+ fprintf(fid, '%15.8f\n', tsai_calib.T);
+ fprintf(fid, '# inverse rotation matrix Rinv\n');
+ fprintf(fid, '%12.8f\t%12.8f\t%12.8f\n', (tsai_calib.Rinv)');
+ fprintf(fid, '# inverse translation vector Tinv\n');
+ fprintf(fid, '%15.8f\n', tsai_calib.Tinv);
+ fprintf(fid, '# rms distance between particle centers found on image plane and their projections\n');
+ fprintf(fid, '# %15.8f\t\t# err_x\n', tsai_calib.err_x);
+ fprintf(fid, '# %15.8f\t\t# err_y\n', tsai_calib.err_y);
+ fprintf(fid, '# %15.8f\t\t# err_t\n', tsai_calib.err_t);
+ fprintf(fid, '\n');
+end
+% laser beam parameters
+fprintf(fid, '##### laser beam #####\n');
+fprintf(fid, '\n');
+fprintf(fid, '0\t\t# finite volume illumination\n');
+fprintf(fid, '-50\t\t# illum_xmin\n');
+fprintf(fid, '+50\t\t# illum_xmax\n');
+fprintf(fid, '-50\t\t# illum_ymin\n');
+fprintf(fid, '+50\t\t# illum_ymax\n');
+fprintf(fid, '-50\t\t# illum_zmin\n');
+fprintf(fid, '+50\t\t# illum_zmax\n');
+fprintf(fid, '\n');
+% 3D matching parameters
+fprintf(fid, '#### thresholds for 3D matching ####\n');
+fprintf(fid, '\n');
+fprintf(fid, '3.0\t\t# mindist_pix (pixel)\n');
+fprintf(fid, '1.0\t\t# maxdist_3D (mm)\n');
+fclose(fid);
+
+fprintf('done\n');
\ No newline at end of file
diff --git a/calibration/calpts2mat.m b/calibration/calpts2mat.m
new file mode 100644
index 0000000000000000000000000000000000000000..e8a089f07c016d2077052112b18a6789ef240ede
--- /dev/null
+++ b/calibration/calpts2mat.m
@@ -0,0 +1,82 @@
+%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
+% load a text file from Jan/Holly's calibration
+% containing indices/image coordinates of calibration points
+% and convert this to the common .MAT file format
+
+addpath([pwd '/pinhole/']);
+
+%% input parameters
+calib_dir = '/nfs/scratch21/lawson/zugspitze/calib/';
+mat_file_pat = '%s/CAM%d-%02d.mat';
+calpts_file_pat = '%s/CalibPts%d.txt';
+
+camera_index = 3;
+camera_id = camera_index - 1;
+grid_dx = 0.5; % real units, mm
+grid_dy = 0.5;
+
+img_width = 1280;
+img_height = 800;
+i_axis = 0 : 1 : img_width - 1;
+j_axis = 0 : 1 : img_height - 1;
+
+DO_DEBUG_PLOT = true;
+
+%% load calibration points file
+fname = sprintf(calpts_file_pat, calib_dir, camera_id);
+C = textread(fname);
+gl_markers = C(:, 1:3)';
+ij_markers = C(:, [5 4])';
+
+%% get plate pos from z
+plate_pos = unique(gl_markers(3,:))';
+n_views = length(plate_pos);
+plate_pos = [zeros(n_views,2), plate_pos];
+
+%% iterate over planes
+for calib_idx = 1 : n_views
+ %% load subset belonging to this plane
+ rng = gl_markers(3,:) == plate_pos(calib_idx, 3);
+ xy_mark = gl_markers(1:2, rng);
+ ij_mark = ij_markers(:, rng);
+ gl_plate_pos = plate_pos(calib_idx,:);
+
+ %% fit pinhole
+ [P2D, ij_res] = p2d_fit(xy_mark, ij_mark);
+ ij_rms = rms(ij_res, 2);
+
+ %% axes and limits
+ x_lim = [min(xy_mark(1,:)), max(xy_mark(1,:))];
+ y_lim = [min(xy_mark(2,:)), max(xy_mark(2,:))];
+ Nx = img_width;
+ Ny = img_height;
+ x_axis = linspace(x_lim(1), x_lim(2), Nx);
+ y_axis = linspace(y_lim(1), y_lim(2), Ny);
+
+ %% plot
+ if DO_DEBUG_PLOT
+ figure(1);
+ clf;
+ plot3(xy_mark(1,:), xy_mark(2,:), ij_mark(1,:), 'b.');
+ hold on;
+ plot3(xy_mark(1,:), xy_mark(2,:), ij_mark(2,:), 'r.');
+ end
+
+ %% save
+ output_file = sprintf(mat_file_pat, calib_dir, camera_index, calib_idx);
+ fprintf('Saving to %s... ', output_file);
+ fstruct = struct('plate_pos', gl_plate_pos, ...
+ 'P', P2D, ...
+ 'i_axis', i_axis, 'j_axis', j_axis, ...
+ 'x_axis', x_axis, 'y_axis', y_axis, 'x_lim', x_lim, 'y_lim', y_lim, ...
+ 'grid_dx', grid_dx, 'grid_dy', grid_dy, ...
+ 'ij_markers', ij_mark, 'xy_markers', xy_mark, ...
+ 'camera_index', camera_index, ...
+ 'id', camera_id);
+ save(output_file, '-struct', 'fstruct');
+ fprintf('done\n');
+
+ %% report
+ fprintf('2D fit plane %d:\n', calib_idx);
+ fprintf(' RMS: %0.3f %0.3f px\n', ij_rms);
+end
\ No newline at end of file
diff --git a/calibration/camera_calibration.m b/calibration/camera_calibration.m
new file mode 100644
index 0000000000000000000000000000000000000000..f0831384ffe2b0b30d0ff0b5ded531940170fdef
--- /dev/null
+++ b/calibration/camera_calibration.m
@@ -0,0 +1,280 @@
+%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
+% camera_calibration.m
+%
+% create a pinhole camera calibration for one or more cameras
+% from a set of views of the same calibration plate in different positions
+% a compensation is made for error in calibration plate position and
+% orientation
+
+addpath([pwd '/pinhole/']);
+addpath([pwd '/../../misc/']);
+addpath([pwd '/../../misc/mtimesx/']);
+addpath([pwd '/../../misc/mmx'])
+addpath([pwd '/../matching/']);
+
+%% input parameters
+
+calib_dir = '/nfs/scratch23/lawson/K62/170203/calib/';
+cam_calib_file_pat = [calib_dir '/CAM%d-%02d.mat'];
+cam_model_file = [calib_dir '/camera-model-original.mat'];
+
+cam_index_range = [1 2];
+cam_view_range = 1 : 7;
+n_views = length(cam_view_range);
+n_cameras = length(cam_index_range);
+n_iterations = 2;
+
+DO_DEBUG_PLOT = true;
+
+%% estimate of plate orientation relative to global coordinate system
+plate_eu = [1 0 0];
+plate_ev = [0 1 0];
+plate_ew = [0 0 1];
+pl_to_gl = [plate_eu(:) plate_ev(:) plate_ew(:)];
+
+%% build setup structure
+
+fprintf('Loading data from file ... ');
+
+gl_plate_pos = zeros(3, n_views);
+pl_to_gl_mat = repmat(pl_to_gl, [1 1 n_views]);
+camera = struct('i_axis', [], 'j_axis', [], ...
+ 'id', [], 'view', []);
+camera.view = struct('uv_markers', [], ...
+ 'ij_markers', []);
+camera.view(1:n_views) = camera.view;
+emptyCamera = camera;
+
+setup = struct('pl_to_gl', pl_to_gl, ...
+ 'pl_to_gl_mat', pl_to_gl_mat, ...
+ 'gl_plate_pos', gl_plate_pos, ...
+ 'omega', eye(3), ...
+ 'gl_plate_dx', zeros(3, n_views), ...
+ 'gl_plate_omega', zeros(3, n_views), ...
+ 'n_cameras', n_cameras, ...
+ 'n_views', n_views, ...
+ 'camera', camera);
+
+for cam_idx = 1 : n_cameras
+ camera = emptyCamera;
+
+ for view_idx = 1 : n_views
+ calib_file = sprintf(cam_calib_file_pat, cam_index_range(cam_idx), cam_view_range(view_idx));
+ f = importdata(calib_file);
+
+ camera.view(view_idx) = ...
+ struct('uv_markers', f.xy_markers, ...
+ 'ij_markers', f.ij_markers);
+ end
+
+ camera.id = f.id;
+ camera.i_axis = f.i_axis;
+ camera.j_axis = f.j_axis;
+ setup.camera(cam_idx)= camera;
+end
+
+for view_idx = 1 : n_views
+ calib_file = sprintf(cam_calib_file_pat, cam_index_range(cam_idx), cam_view_range(view_idx));
+ f = importdata(calib_file);
+ gl_plate_pos(:, view_idx) = f.plate_pos;
+end
+% enforce barycentre of plate positions to be zero
+gl_plate_pos = bsxfun(@minus, gl_plate_pos, mean(gl_plate_pos,2));
+setup.gl_plate_pos = gl_plate_pos;
+fprintf('done\n');
+
+%% pinhole fit before bundle calibration
+fprintf('fitting camera model to setup before bundle calibration:\n');
+[s_old_calib,res_before]= pinhole_camera_fit(setup);
+fprintf('fit quality before bundle calibration:\n');
+for cam_idx = 1 : n_cameras
+
+ fprintf(' camera %d:\n', cam_idx);
+ fprintf(' RMS of i residual: %0.3f px\n', rms(res_before{cam_idx}(1,:)));
+ fprintf(' RMS of j residual: %0.3f px\n', rms(res_before{cam_idx}(2,:)));
+ %fprintf(' RMS of x residual: %0.3f mm\n', rms(
+ setup.camera(cam_idx).calib = s_old_calib(cam_idx);
+end
+old_setup = setup;
+
+%% iterate error fitting
+% 1) use pinhole model to esimate error in plate position
+% 2) update model of plate position
+% 3) fit a pinhole camera model
+% 4) go to 1 if iteration limit not exceeded
+for it = 1 : n_iterations
+ %% calculate model of error
+ fprintf(' fitting model of error ... ');
+ new_setup = traverse_error_fit(setup);
+ fprintf('done\n');
+
+ %% apply correction
+ new_setup.gl_plate_pos = new_setup.gl_plate_pos + new_setup.gl_plate_dx;
+ %new_setup.pl_to_gl = new_setup.omega * new_setup.pl_to_gl;
+ for v = 1 : n_views
+ omega_vec = new_setup.gl_plate_omega(:,v);
+ Omega = vrrotvec2mat([omega_vec(:)/(norm(omega_vec)+eps); norm(omega_vec)]);
+ new_setup.pl_to_gl_mat(:,:,v) = Omega * new_setup.pl_to_gl_mat(:,:,v);
+ end
+ new_setup.omega = eye(3);
+ new_setup.gl_plate_dx = zeros(3, n_views);
+ new_setup.gl_plate_omega= zeros(3, n_views);
+
+ %% calculate new camera calibration
+ % assume no perturbation to plate position
+ fprintf('Iteration %d of %d\n', it, n_iterations);
+ fprintf(' fitting pinhole camera models ... \n');
+ [s_cam_calib, res] = pinhole_camera_fit(new_setup);
+
+ for cam_idx = 1 : n_cameras
+ fprintf(' camera %d:\n', cam_idx);
+ fprintf(' RMS of i residual: %0.3f\n', rms(res{cam_idx}(1,:)));
+ fprintf(' RMS of j residual: %0.3f\n', rms(res{cam_idx}(2,:)));
+ end
+
+ %% fix reference frame
+ % stop setup shuffling about by enforcing barycentre of plate positions
+ % to be origin
+ origin_shift = -mean(new_setup.gl_plate_pos, 2);
+ new_setup.gl_plate_pos = bsxfun(@plus, new_setup.gl_plate_pos, origin_shift);
+ for cam_idx = 1 : n_cameras
+ calib = s_cam_calib(cam_idx);
+ calib.xc = calib.xc(:) + origin_shift;
+ calib.P = calib.G * calib.R * [eye(3), -calib.xc(:)];
+ new_setup.camera(cam_idx).calib = calib;
+ end
+
+ %% update
+ setup = new_setup;
+end
+new_setup = setup;
+
+%% pinhole fit for final setup
+fprintf('fitting camera model to new setup:\n');
+[s_new_calib, res_after]= pinhole_camera_fit(new_setup);
+fprintf('fit quality after bundle calibration:\n');
+for cam_idx = 1 : n_cameras
+ fprintf(' camera %d:\n', cam_idx);
+ fprintf(' RMS of i residual: %0.3f\n', rms(res_after{cam_idx}(1,:)));
+ fprintf(' RMS of j residual: %0.3f\n', rms(res_after{cam_idx}(2,:)));
+ setup.camera(cam_idx).calib = s_new_calib(cam_idx);
+end
+
+%% determine size + center of volume
+% bounding volume
+[gl_origin, r_sphere] = volume_bounds(new_setup);
+r_sphere_lim = min(r_sphere);
+
+% add to setup
+new_setup.gl_origin = gl_origin;
+new_setup.r_sphere = r_sphere;
+new_setup.r_sphere_lim = r_sphere_lim;
+
+%% difference compared to old setup
+gl_plate_dx = new_setup.gl_plate_pos - old_setup.gl_plate_pos;
+gl_plate_omega = zeros(4, n_views);
+for v = 1 : n_views
+ Omega = new_setup.pl_to_gl_mat(:,:,v) / old_setup.pl_to_gl_mat(:,:,v);
+ gl_plate_omega(:,v) = vrrotmat2vec(Omega);
+end
+gl_plate_dtheta = gl_plate_omega(4,:);
+gl_plate_rotaxis = gl_plate_omega(1:3,:);
+
+%% report
+fprintf('------------------------------------------------------------------\n');
+fprintf('Traverse position error modelling complete\n');
+fprintf('\n');
+for c = 1 : n_cameras
+ fprintf('Camera %d\n', c);
+ fprintf('Before fit: %0.3f %0.3f px rms %0.3f %0.3f px max\n', ...
+ rms(res_before{c},2), max(abs(res_before{c}),[],2) );
+ fprintf('After fit: %0.3f %0.3f px rms %0.3f %0.3f px max\n', ...
+ rms(res_after{c},2), max(abs(res_after{c}),[],2) );
+ fprintf('Largest inscribed sphere:\n');
+ fprintf(' r = %0.2f mm, d = %0.2fmm\n', r_sphere(c), r_sphere(c)*2);
+ fprintf('\n');
+end
+fprintf('Largest rotation angle: %0.3f mrad\n', max(abs(gl_plate_dtheta))*1000);
+fprintf('Largest plate dx: %0.3f\n', max(abs(gl_plate_dx(1,:))));
+fprintf('Largest plate dy: %0.3f\n', max(abs(gl_plate_dx(2,:))));
+fprintf('Largest plate dz: %0.3f\n', max(abs(gl_plate_dx(3,:))));
+
+
+% report
+fprintf('\n\n\n');
+fprintf('Origin is at:\n');
+fprintf(' %+0.5f\n', gl_origin);
+fprintf('Inscribed sphere has dimension:\n');
+fprintf(' r = %0.2f mm, d = %0.2f mm\n', r_sphere_lim, 2*r_sphere_lim);
+
+%% save camera model to file
+fprintf('Saving ... ');
+save(cam_model_file, '-struct', 'new_setup');
+fprintf('done\n');
+
+%% plot error in traverse position
+figure(10);
+quiver(gl_plate_pos(1,:), gl_plate_pos(3,:), ...
+ gl_plate_dx(1,:), gl_plate_dx(3,:), 'k-');
+axis equal tight;
+xlabel('Traverse x position / mm');
+ylabel('Traverse z position / mm');
+title('Estimated error in traverse position');
+
+%% plot to compare new and old setups
+if DO_DEBUG_PLOT
+ %% colours for different views
+ cmap = jet;
+ cvec_map = linspace(0,1,size(cmap,1));
+ cvec_int = linspace(0,1,n_views);
+ marker_colour = zeros(n_views,3);
+ for rgb = 1 : 3
+ marker_colour(:,rgb) = interp1(cvec_map, cmap(:,rgb), cvec_int, 'linear');
+ end
+ marker_colour(1,:) = [1 0 0];
+ marker_colour(end,:) = [0 0 1];
+
+ %% loop over setups
+ setup_ary = cell(1,2);
+ setup_ary{1} = old_setup;
+ setup_ary{2} = new_setup;
+
+ for s = 1 : 2
+ %% initialise
+ setup = setup_ary{s};
+ figure(s);
+ clf;
+ subplot1(2,2,'Gap',[0.05 0.05]);
+ %% plot each camera
+ for cam_idx = 1 : n_cameras
+ subplot1(cam_idx);
+ cla;
+ hold on;
+
+ camera = setup.camera(cam_idx);
+ for v = n_views%[1 , n_views]
+ uv_mark = camera.view(v).uv_markers;
+ n_mark = size(uv_mark, 2);
+ gl_plate = setup.gl_plate_pos(:,v);
+ gl_mark = bsxfun(@plus, gl_plate, setup.pl_to_gl_mat(:,:,v)*[uv_mark; zeros(1,n_mark)]);
+
+ ij_mark = camera.view(v).ij_markers;
+ ij_proj = pinhole_model(gl_mark, camera.calib);
+
+ plot(ij_mark(1,:), ij_mark(2,:), 'o', 'color', 'k'); %marker_colour(v,:));
+ plot(ij_proj(1,:), ij_proj(2,:), 'x', 'color', 'r'); %marker_colour(v,:));
+ %quiver( ij_mark(1,:), ij_mark(2,:), ...
+ % ij_proj(1,:)-ij_mark(1,:), ij_mark(2,:)-ij_proj(2,:),'k-');
+ end
+ axis equal tight xy;
+ xlim(sort(camera.i_axis([1 end])));
+ ylim(sort(camera.j_axis([1 end])));
+ make_plot_pretty('i / px', 'j / px', sprintf('Camera %d', cam_index_range(cam_idx)), 20);
+ if cam_idx ~= 1
+ ylabel('');
+ end
+ set(gca,'xtick',camera.i_axis(1):256:camera.i_axis(end));
+ set(gca,'ytick',camera.j_axis(1):256:camera.j_axis(end));
+ end
+ end
+end
\ No newline at end of file
diff --git a/calibration/check_calibration.m b/calibration/check_calibration.m
new file mode 100644
index 0000000000000000000000000000000000000000..ccc4b0ba5074ef4c3e2c873312b33d3e834de8ba
--- /dev/null
+++ b/calibration/check_calibration.m
@@ -0,0 +1,120 @@
+%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
+%
+% check calibration by loading and dewarping particle images
+%
+
+addpath([pwd '/../reconstruction/']);
+
+%% input parameters
+
+omega_y_mrad = -40;
+
+% image to dewarp and plot
+cam_index = 2;
+figure_index = 100*cam_index;
+sheet_index_range = 20;%[11:14];
+volume_index = 1;
+series_index = 1;
+
+n_sheets = length(sheet_index_range);
+
+% calibration
+calibration_file = 'F:/bigtank-jul-2013/CAL-06-08-13/scanning-setup.mat';
+
+% image processing options
+DO_IMAGE_FLIPUPDOWN = 0;
+DO_IMAGE_FLIPLEFTRIGHT = 0;
+DO_DARK_IMAGE = 1;
+image_clim = [0 4];
+
+% path of files to reconstruct
+source_img_dir = 'F:/bigtank-jul-2013/MEAS-06-08-13/SCAN-05/';
+dark_img_dir = 'F:/bigtank-jul-2013/DARK/MEAS-06-08-13/SCAN-05/';
+source_img_file_pat = 'CAMID%d-series-%02d-volume-%02d-frame-%02d.raw12';
+dark_img_file_pat = 'CAMID%d-frame-%02d.raw12';
+
+source_file_name_fun = @(camid,series,volume,frame) sprintf(['%s/' source_img_file_pat], source_img_dir, camid, series, volume, frame);
+dark_file_name_fun = @(camid,frame) sprintf(['%s/' dark_img_file_pat], dark_img_dir, camid, frame);
+
+%% load the calibration data
+fprintf('------------------------------------------------------------------\n');
+fprintf('CALIBRATION:\n');
+
+fstruct = importdata(calibration_file);
+% camera id
+camera_id = fstruct.camera_id(cam_index);
+% coordinate system and axes
+volume_x = fstruct.X;
+volume_y = fstruct.Y;
+volume_z = fstruct.Z;
+volume_Lx = volume_x(end) - volume_x(1);
+volume_Ly = volume_y(end) - volume_y(1);
+volume_Lz = volume_z(end) - volume_z(1);
+volume_Nx = length(volume_x);
+volume_Ny = length(volume_y);
+volume_Nz = length(volume_z);
+% camera mapping functions
+sCamCalib = fstruct.camera_calibration(cam_index);
+re_plane_coeffs = fstruct.re_plane_coeffs;
+% image coordinate system
+im_i = fstruct.i;
+im_j = fstruct.j;
+
+
+%% modify plane normal
+
+re_plane_normal = re_plane_coeffs(:, 1:3);
+Omega = vrrotvec2mat([0 1 0 omega_y_mrad/1000]);
+re_plane_normal = (Omega*re_plane_normal')';
+re_plane_coeffs(:,1:3) = re_plane_normal;
+
+%% read the images from file
+fprintf(' --- loading images ... ');
+source_file_names = arrayfun(source_file_name_fun, ones(1,n_sheets)*camera_id, ...
+ ones(1,n_sheets)*series_index, ...
+ ones(1,n_sheets)*volume_index, ...
+ sheet_index_range, 'UniformOutput', false);
+[imgvol, fail] = load_image_array(source_file_names, DO_IMAGE_FLIPUPDOWN, DO_IMAGE_FLIPLEFTRIGHT);
+
+if DO_DARK_IMAGE
+ dark_file_names = arrayfun( dark_file_name_fun, ones(1, n_sheets)*camera_id, ...
+ sheet_index_range, 'UniformOutput', false);
+ darkvol = load_image_array(dark_file_names, DO_IMAGE_FLIPUPDOWN, DO_IMAGE_FLIPLEFTRIGHT);
+ % subtract dark images
+ imgvol = max(imgvol - darkvol, 0);
+end
+
+fprintf('done\n');
+
+%% dewarp each image
+% back-projection onto planes with regular x and y
+% (z coordinate determined by plane of laser sheet)
+imgvol_dewarped = zeros(volume_Nx, volume_Ny, n_sheets);
+
+[xmat, ymat] = ndgrid(volume_x, volume_y);
+
+for it = 1 : n_sheets
+ %% calculate z coords on this plane and get image coords
+ sheet_index = sheet_index_range(it);
+ M = re_plane_coeffs(sheet_index, :);
+ zmat = -(M(4) + M(1)*xmat + M(2)*ymat) / M(3);
+ xyzmat = [xmat(:) ymat(:) zmat(:)]';
+ ij = pinhole_model(xyzmat, sCamCalib);
+
+ %% load image and interpolate
+ im = imgvol(:, :, it);
+ im_int = interp2(im_i, im_j, im', ij(1,:), ij(2,:), 'linear', nan);
+ im_int = reshape(im_int, [volume_Nx volume_Ny]);
+ imgvol_dewarped(:,:,it) = im_int;
+end
+
+%% display
+
+for it = 1 : n_sheets
+ figure(figure_index + sheet_index_range(it));
+ hold off;
+ im = imgvol_dewarped(:,:,it);
+ imagesc(volume_x, volume_y, im' / rms(im(:)));
+ axis equal tight xy;
+ set(gca,'clim',image_clim);
+end
diff --git a/calibration/combine_calibrations.m b/calibration/combine_calibrations.m
new file mode 100644
index 0000000000000000000000000000000000000000..493c4dc875529def2f1b67f9f739b734ea4c70ed
--- /dev/null
+++ b/calibration/combine_calibrations.m
@@ -0,0 +1,309 @@
+%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
+% combine_c.m
+%
+% create a pinhole camera calibration for one or more cameras
+% from a set of views of the same calibration plate in different positions
+% a compensation is made for error in calibration plate position and
+% orientation
+
+addpath([pwd '/pinhole/']);
+
+%% input parameters
+
+calib_dir = 'D:/bigtank-jul-2013/CAL-20-07-13/NEWSHEET/';
+cam_model_file = [calib_dir '/camera-model-280415.mat'];
+sheet_model_file = [calib_dir '/sheet-model-280415.mat'];
+setup_file = [calib_dir '/scanning-setup-280415.mat'];
+poly_order = 5;
+
+%% load camera calibration
+
+fstruct = importdata(cam_model_file);
+n_cameras = fstruct.n_cameras;
+camera_setup = fstruct.camera;
+im_i = camera_setup(1).i_axis;
+im_j = camera_setup(1).j_axis;
+
+%% load laser sheet calibration
+
+fstruct = importdata(sheet_model_file);
+n_sheets = fstruct.n_sheets;
+gl_eZ_mat = fstruct.gl_sheet_normal;
+gl_xs_mat = fstruct.gl_sheet_pos;
+gl_on_sheet = fstruct.gl_on_sheet;
+w_vec = fstruct.sheet_width;
+
+%% define orientation of [X,Y,Z] coordinate system
+
+% orentation of reconstruction coordinate system in global system
+gl_eZ = mean(gl_eZ_mat, 1);
+gl_eX = cross([0 1 0]', gl_eZ);
+gl_eX = gl_eX / norm(gl_eX);
+gl_eY = cross(gl_eZ, gl_eX);
+
+% transfer matrices for gl->re and re->gl orientation
+re_to_gl = [gl_eX(:), gl_eY(:), gl_eZ(:)];
+gl_to_re = re_to_gl^-1;
+
+% origin of reconstruction coordinate system
+gl_X0 = [0 0 0]'; % origin in gl coordinates
+re_X0 = gl_to_re * gl_X0; % origin in re
+re_eZ_mat = (gl_to_re * gl_eZ_mat')';
+re_xs_mat = (gl_to_re * (gl_xs_mat - ones(n_sheets,1)*gl_X0(:)')')';
+
+%% calculate the plane coefficients
+% these are a set of 4 coefficients per plane, M(1:4)
+% all points [X,Y,Z] on this plane satisfy XM(1) + YM(2) + ZM(3) + M(4) = 0
+
+re_plane_coeffs = zeros(n_sheets, 4);
+
+for sheet_idx = 1:n_sheets
+ % get equation of plane dot(M,X) = 1
+ gl_sheet_xs = gl_xs_mat(sheet_idx, :)';
+ gl_sheet_en = gl_eZ_mat(sheet_idx, :)';
+ re_sheet_xs = gl_to_re * (gl_sheet_xs - gl_X0);
+ re_sheet_en = gl_to_re * gl_sheet_en;
+ re_sheet_en = re_sheet_en / norm(re_sheet_en);
+ % save to matrix
+ re_plane_coeffs(sheet_idx,1:3) = re_sheet_en;
+ re_plane_coeffs(sheet_idx,4) = -dot(re_sheet_xs, re_sheet_en);
+end
+
+% smooth out errors by replacing measured value with fitted value
+k = 0 : n_sheets - 1;
+re_plane_coeffs_o = re_plane_coeffs;
+for i = 1:4
+ p = polyfit(k, re_plane_coeffs(:,i)', poly_order);
+ re_plane_coeffs(:,i) = polyval(p, k);
+end
+% fix coefficients so M(1:3) is a unit vector
+for i = 1 : n_sheets
+ en = re_plane_coeffs(i,1:3);
+ d = re_plane_coeffs(i,4);
+ re_plane_coeffs(i,:) = [en d] / norm(en);
+end
+
+sheet_spacing = polyfit(k, re_plane_coeffs(:,4)', 1);
+sheet_spacing = abs(sheet_spacing(1));
+
+%% convert camera calibration to reconstruction coordinate system
+% incorporates shift of origin and rotation
+
+for cam_idx = 1 : n_cameras
+ gl_pinhole_model = camera_setup(cam_idx).calib;
+ gl_G = gl_pinhole_model.G;
+ gl_R = gl_pinhole_model.R;
+ gl_xc = gl_pinhole_model.xc;
+ re_R = gl_R * re_to_gl;
+ re_xc = gl_to_re * (gl_xc(:) - gl_X0(:));
+ re_P = gl_G * re_R * [eye(3) -re_xc];
+
+ re_pinhole_model = gl_pinhole_model;
+ re_pinhole_model.P = re_P;
+ re_pinhole_model.R = re_R;
+ re_pinhole_model.xc = re_xc;
+ camera_setup(cam_idx).calib = re_pinhole_model;
+ camera_setup(cam_idx).gl_calib= gl_pinhole_model;
+end
+
+%% calculate axes for reconstruction volume
+% with knowledge of each camera's pinhole model
+% and the laser sheet setup
+% we can define axes for the reconstruction volume
+
+X_lim_mat = zeros(n_cameras, 2);
+Y_lim_mat = zeros(n_cameras, 2);
+Z_lim_mat = zeros(n_cameras, 2);
+
+ilim = [im_i(3) im_i(end-2)];
+jlim = [im_j(3) im_j(end-2)];
+
+% report on what we are about to do
+fprintf('------------------------------------------------------------------\n');
+fprintf('CAMERA CALIBRATION\n');
+fprintf('Number of cameras: %d\n', n_cameras);
+
+for cam_idx = 1 : n_cameras
+ %% find the extent of the visible volume
+ % the largest possible volume, bound by first and last laser sheets
+ % which projects onto the space between ilim(1) < i < ilim(2)
+ % and jlim(1) < j < lim(2)
+ M1 = re_plane_coeffs(1, :);
+ Mend = re_plane_coeffs(end, :);
+ camera_calib = camera_setup(cam_idx).calib;
+ P = camera_calib.P;
+ [XLim,YLim,ZLim] = find_largest_volume(P, ilim, jlim, M1, Mend);
+ % add a little extra bit of space in reconstruction around edges in z
+ % direction
+ ZLim = ZLim + sheet_spacing*[-2 2]';
+ % save
+ X_lim_mat(cam_idx,:)= XLim;
+ Y_lim_mat(cam_idx,:)= YLim;
+ Z_lim_mat(cam_idx,:)= ZLim;
+
+ %% report for this camera
+ fprintf('Camera %d, ID = %d\n', cam_idx, camera_setup(cam_idx).id);
+ fprintf('X limits: %0.1f to %0.1f\n', XLim(1), XLim(2));
+ fprintf('Y limits: %0.1f to %0.1f\n', YLim(1), YLim(2));
+ fprintf('Z limits: %0.1f to %0.1f\n', ZLim(1), ZLim(2));
+ fprintf('Camera position: %0.2f %0.2f %0.2f\n', camera_calib.xc);
+ fprintf('Internal camera parameters:\n');
+ disp(camera_calib.G);
+end
+
+%% define a coordinate system visible to all cameras
+X_lim = [max(X_lim_mat(:,1)) min(X_lim_mat(:,2))];
+Y_lim = [max(Y_lim_mat(:,1)) min(Y_lim_mat(:,2))];
+Z_lim = [max(Z_lim_mat(:,1)) min(Z_lim_mat(:,2))];
+% fix resolution
+re_dY = (Y_lim(2) - Y_lim(1)) / (jlim(2) - jlim(1));
+re_dX = re_dY;
+re_dZ = re_dX;
+re_X = X_lim(1) : re_dX : X_lim(2);
+re_Y = Y_lim(1) : re_dY : Y_lim(2);
+re_Z = Z_lim(1) : re_dZ : Z_lim(2);
+
+fprintf('------------------------------------------------------------------\n');
+fprintf('Combined calibration:\n');
+fprintf('X limits: %0.1f to %0.1f (%d vox)\n', X_lim(1), X_lim(2), length(re_X));
+fprintf('Y limits: %0.1f to %0.1f (%d vox)\n', Y_lim(1), Y_lim(2), length(re_Y));
+fprintf('Z limits %0.1f to %0.1f (%d vox)\n', Z_lim(1), Z_lim(2), length(re_Z));
+fprintf('voxel size: %0.2f x %0.2f x %0.2f\n', re_dX, re_dY, re_dZ);
+
+%% save to file
+
+fprintf('------------------------------------------------------------------\n');
+fprintf('Multiple camera calibration complete\n');
+fprintf('Saving to file ... ');
+
+fstruct = struct('n_sheets', n_sheets, ...
+ 'gl_sheet_normal', gl_eZ_mat, 're_sheet_normal', re_eZ_mat, ...
+ 'gl_sheet_pos', gl_xs_mat, 're_sheet_pos', re_xs_mat, ...
+ 're_plane_coeffs', re_plane_coeffs, ...
+ 'sheet_width', w_vec, ...
+ 'gl_X0', gl_X0, ...
+ 'gl_to_re', gl_to_re, ...
+ 'i', im_i, 'j', im_j, ...
+ 'X', re_X, 'Y', re_Y, 'Z', re_Z, ...
+ 'n_cameras', n_cameras, ...
+ 'camera_calibration', [camera_setup.calib], ...
+ 'camera_id', [camera_setup.id]);
+save(setup_file, '-struct', 'fstruct');
+fprintf('done\n');
+
+%% plot residuals of fit for sheet coefficients
+figure(1);
+clf;
+plot(k, re_plane_coeffs_o(:,1) - re_plane_coeffs(:,1), 'r.', ...
+ k, re_plane_coeffs_o(:,2) - re_plane_coeffs(:,2), 'g.', ...
+ k, re_plane_coeffs_o(:,3) - re_plane_coeffs(:,3), 'b.');
+xlabel('sheet index');
+ylabel('coefficient residual (unit vectors)');
+title('residuals for polynomial fit of laser sheet normal');
+
+figure(2);
+plot(k, (re_plane_coeffs_o(:,4) - re_plane_coeffs(:,4))/re_dX, 'k.', ...
+ [0 n_sheets-1], [1 1]*+0.01 * sheet_spacing / re_dX, 'k--', ...
+ [0 n_sheets-1], [1 1]*-0.01 * sheet_spacing / re_dX, 'k--');
+ylim(sheet_spacing * [-1 1]*0.1 / re_dX);
+xlabel('sheet index');
+ylabel('coefficient residual (normal distance) / vx');
+title('residuals for polynomial fit of laser sheet position');
+
+%% plot projection of reconstruction volume in each view
+
+figure(9);
+clf;
+for cam_idx = 1 : n_cameras
+ %% create figure
+ subplot(1,n_cameras,cam_idx);
+ cla;
+
+ %% plot corners
+ camera_calib = camera_setup(cam_idx).calib;
+ corners_xyz = [X_lim([1 1 1 1 2 2 2 2]);
+ Y_lim([1 1 2 2 1 1 2 2]);
+ Z_lim([1 2 1 2 1 2 1 2])];
+ corners_ij = pinhole_model(corners_xyz, camera_calib);
+ plot( corners_ij(1,[1 2 3 4 1]), corners_ij(2, [1 2 3 4 1]), 'k.-', ...
+ corners_ij(1,[5 6 7 8 5]), corners_ij(2, [5 6 7 8 5]), 'k.-', ...
+ corners_ij(1,[1 3 7 5 1]), corners_ij(2, [1 3 7 5 1]), 'k.-', ...
+ corners_ij(1,[2 4 8 6 2]), corners_ij(2, [2 4 8 6 2]), 'k.-');
+ % plot without pinhole model
+ hold on
+ camera_calib.kr = [0 0];
+ camera_calib.ic = [512 512];
+ corners_ij = pinhole_model(corners_xyz, camera_calib);
+ plot( corners_ij(1,[1 2 3 4 1]), corners_ij(2, [1 2 3 4 1]), 'r.-', ...
+ corners_ij(1,[5 6 7 8 5]), corners_ij(2, [5 6 7 8 5]), 'r.-', ...
+ corners_ij(1,[1 3 7 5 1]), corners_ij(2, [1 3 7 5 1]), 'r.-', ...
+ corners_ij(1,[2 4 8 6 2]), corners_ij(2, [2 4 8 6 2]), 'r.-');
+ %% make pretty
+ axis equal tight xy;
+ xlim(im_i([1 end]));
+ ylim(im_j([1 end]));
+ xlabel('i');
+ ylabel('j');
+ title(sprintf('Camera %d, ID %d', cam_idx, camera_setup(cam_idx).id));
+end
+
+
+%% plot in reconstruction coordinate system
+
+figure(10);
+clf;
+hold off;
+
+% plot planes
+M1 = re_plane_coeffs(1,:);
+Mend = re_plane_coeffs(end,:);
+x_corners = [X_lim(1) X_lim(1) X_lim(2) X_lim(2) X_lim(1)];
+y_corners = [Y_lim(1) Y_lim(2) Y_lim(2) Y_lim(1) Y_lim(1)];
+% first plane
+z_corners = -(M1(1)*x_corners + M1(2)*y_corners + M1(4)) / M1(3);
+P1 = patch(x_corners, y_corners, z_corners, 'g');
+
+% last plane
+z_corners = -(Mend(1)*x_corners + Mend(2)*y_corners + Mend(4)) / Mend(3);
+Pend = patch(x_corners, y_corners, z_corners, 'g');
+set([P1 Pend], 'FaceAlpha', 0.5, 'edgealpha', 1);
+
+hold on;
+
+% plot viewing frustum of each camera
+ilim = [im_i(1) im_i(end)];
+jlim = [im_j(1) im_j(end)];
+i_corners = [ilim(1) ilim(1) ilim(2) ilim(2)];
+j_corners = [jlim(1) jlim(2) jlim(2) jlim(1)];
+ij_corners = [i_corners; j_corners];
+
+str_colour = {'r','k','b'};
+
+for cam_idx = 1 : n_cameras
+ calib = camera_setup(cam_idx).calib;
+ Gc = calib.G;
+ Rc = calib.R;
+ xc = calib.xc;
+ ic = calib.ic;
+ kr = calib.kr;
+
+ % convert dewarped image coordinates to pinhole image coordinates
+ ij_undist= inverse_radial_distortion(ij_corners, ic, kr);
+ for corner = 1 : 4
+ ytilde= [ij_undist(:, corner); 1];
+ e_ray = Rc^-1 * Gc^-1 * ytilde;
+ e_ray = e_ray / norm(e_ray);
+
+ x_far = xc + e_ray * norm(xc) * 1.2;
+ x_near= xc;
+
+ plot3([x_near(1) x_far(1)], ...
+ [x_near(2) x_far(2)], ...
+ [x_near(3) x_far(3)], '-', 'Color', str_colour{cam_idx});
+ end
+end
+
+axis equal;% tight xy;
+xlim([-100 100]);
+ylim([-100 100]);
+zlim([-100 100]);
\ No newline at end of file
diff --git a/calibration/combine_markers.m b/calibration/combine_markers.m
new file mode 100644
index 0000000000000000000000000000000000000000..fa31d1ce34e7a15abe5c27a68c03943f5790a806
--- /dev/null
+++ b/calibration/combine_markers.m
@@ -0,0 +1,316 @@
+%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
+% combine_markers.m
+%
+% combine calibration images of the same markers from a single camera
+%
+addpath([pwd '/../../piv2d/']);
+addpath([pwd '/pinhole/']);
+
+%% input parameters
+
+calibration_dir = '/nfs/data/lawson/160715/calib/';
+calib_infile_pat = 'CAM2-%02d.mat';
+calib_outfile_pat = 'C-CAM2-%02d.mat';
+calib_file_range = 1 : 16;
+peak_mag_threshold= 0.5;
+
+marker_size = [0.25 0.25]; % mm, interpreted as half-size
+marker_min_bright = 50; % minimum brightness of marker image
+
+%% load calibration files
+
+x_lim = [inf -inf];
+y_lim = [inf -inf];
+n_calibrations = length(calib_file_range);
+
+fprintf('------------------------------------------------------------------\n');
+fprintf('Loading %d calibration files ... ', n_calibrations);
+
+for file_idx = calib_file_range
+ file_name = sprintf([calibration_dir '/' calib_infile_pat], file_idx);
+ fstruct = importdata(file_name);
+
+ x_lim = [min([fstruct.x_lim(1) x_lim(1)]) max([fstruct.x_lim(2) x_lim(2)])];
+ y_lim = [min([fstruct.y_lim(1) y_lim(1)]) max([fstruct.y_lim(2) y_lim(2)])];
+end
+
+grid_dx = fstruct.grid_dx;
+grid_dy = fstruct.grid_dy;
+Nx = size(fstruct.source_image, 1);
+Ny = size(fstruct.source_image, 2);
+
+fprintf('done\n');
+
+%% resample images onto common grid
+
+fprintf('Resampling images onto common grid ... ');
+
+x = linspace(x_lim(1), x_lim(2), Nx);
+y = linspace(y_lim(1), y_lim(2), Ny);
+dx = x(2) - x(1);
+dy = y(2) - y(1);
+[xmat, ymat] = ndgrid(x, y);
+
+dewarped_image_mat= zeros(n_calibrations, Nx, Ny);
+for calib_idx = 1 : n_calibrations
+ file_idx = calib_file_range(calib_idx);
+ file_name = sprintf([calibration_dir '/' calib_infile_pat], file_idx);
+ fstruct = importdata(file_name);
+
+ source_image = fstruct.source_image;
+ xytilde = [xmat(:) ymat(:) ones(Nx*Ny,1)]';
+ ijtilde = fstruct.P * xytilde;
+ imat = reshape(ijtilde(1,:) ./ ijtilde(3,:), [Nx Ny]);
+ jmat = reshape(ijtilde(2,:) ./ ijtilde(3,:), [Nx Ny]);
+ i_axis = fstruct.i_axis;
+ j_axis = fstruct.j_axis;
+ dewarped_image = interp2(i_axis, j_axis, source_image', imat, jmat, 'linear', nan);
+
+ dewarped_image_mat(calib_idx, :, :) = dewarped_image;
+end
+
+fprintf('done\n');
+
+%% identify which markers are in which images
+
+x_lim_grid = fix(x_lim/grid_dx) * grid_dx;
+y_lim_grid = fix(y_lim/grid_dy) * grid_dy;
+x_grid = x_lim_grid(1) : grid_dx : x_lim_grid(2);
+y_grid = y_lim_grid(1) : grid_dy : y_lim_grid(2);
+Nx_grid = length(x_grid);
+Ny_grid = length(y_grid);
+
+[x_grid_mat, y_grid_mat] = ndgrid(x_grid, y_grid);
+n_shared_views = zeros(Nx_grid, Ny_grid);
+b_marker_visible = false(Nx_grid, Ny_grid, n_calibrations);
+
+fprintf('Searching for common markers ... ');
+
+for ix = 1 : Nx_grid
+ for iy = 1 : Ny_grid
+ %% find which images contain this marker
+ % extrapolated values in images are nans
+ % so markers we cannot see have nans near them
+ marker_is_visible = false(1, n_calibrations);
+ xrange = abs(x - x_grid(ix)) < marker_size(1);
+ yrange = abs(y - y_grid(iy)) < marker_size(2);
+ for calib_idx = 1 : n_calibrations
+ dewarped_image = dewarped_image_mat(calib_idx, :, :);
+ dewarped_image = squeeze(dewarped_image);
+ marker_image = dewarped_image(xrange,yrange);
+ marker_brightness = mean(marker_image(:));
+ marker_is_visible(calib_idx) = ...
+ all(~isnan(marker_image(:))) && marker_brightness > marker_min_bright;
+ end
+ n_shared_views(ix,iy) = sum(marker_is_visible);
+ b_marker_visible(ix, iy, :) = marker_is_visible;
+ end
+end
+
+fprintf('done\n');
+
+%% report on what markers are common
+b_all_common = n_shared_views == n_calibrations;
+b_some_common = n_shared_views >= 2;
+n_all_common = sum(b_all_common(:));
+n_some_common = sum(b_some_common(:));
+
+fprintf('Found %d markers common across all views\n', n_all_common);
+fprintf('Found %d markers in at least 2 views\n', n_some_common);
+
+sum_of_all_images = mean(dewarped_image_mat, 1);
+sum_of_all_images = squeeze(sum_of_all_images);
+
+figure(1);
+hold off;
+imagesc(x, y, sum_of_all_images');
+hold on;
+plot(x_grid_mat(b_all_common ), y_grid_mat(b_all_common ), 'go');
+plot(x_grid_mat(b_some_common), y_grid_mat(b_some_common), 'rx');
+axis equal tight xy;
+colormap gray;
+xlim(x_lim);
+ylim(y_lim);
+title('Average of all images');
+xlabel('x / mm');
+ylabel('y / mm');
+
+sInput = input('Do you want to check markers? (y): ', 's');
+if strcmp(sInput, 'y')
+ for calib_idx = 1 : n_calibrations
+ dewarped_image = squeeze(dewarped_image_mat(calib_idx, :, :));
+ marker_visible = squeeze(b_marker_visible(:,:,calib_idx));
+
+ figure(1);
+ hold off;
+ imagesc(x, y, dewarped_image');
+ axis equal tight xy;
+ colormap gray;
+ xlim(x_lim);
+ ylim(y_lim);
+ hold on;
+ plot(x_grid_mat(marker_visible), y_grid_mat(marker_visible), 'go');
+
+ fprintf('Image %d of %d\n', calib_idx, n_calibrations);
+ pause();
+ end
+end
+
+%% cross-correlate to find offset
+
+fprintf('Finding optimal shift of markers ... ');
+
+marker_shift_x = zeros(Nx_grid, Ny_grid, n_calibrations);
+marker_shift_y = zeros(Nx_grid, Ny_grid, n_calibrations);
+
+for ix = 1 : Nx_grid
+ for iy = 1 : Ny_grid
+ %% can't make a correction if there are <2 views
+ if n_shared_views(ix, iy) < 2
+ continue;
+ end
+
+ %% get image of marker in first view
+ i_shared_views = find(b_marker_visible(ix, iy, :));
+ xrange = abs(x - x_grid(ix)) < marker_size(1);
+ yrange = abs(y - y_grid(iy)) < marker_size(2);
+ first_view_idx = i_shared_views(1);
+ first_view_img = dewarped_image_mat(first_view_idx, xrange, yrange);
+ first_view_img = squeeze(first_view_img);
+ first_view_img = first_view_img - mean(first_view_img(:));
+ first_view_e = sum(first_view_img(:).^2);
+
+ %% cross correlate to find offset
+ for iv = 2 : n_shared_views(ix, iy)
+ other_view_idx = i_shared_views(iv);
+ other_view_img = dewarped_image_mat(other_view_idx, xrange, yrange);
+ other_view_img = squeeze(other_view_img);
+ other_view_img = other_view_img - mean(other_view_img(:));
+ other_view_e = sum(other_view_img(:).^2);
+
+ xx = xcorr2(first_view_img, other_view_img);
+ xx = xx / sqrt(first_view_e * other_view_e);
+ peak_mag = max(xx(:));
+ peak_loc = PIV_2d_peaklocate(xx);
+
+ % check for sufficient correlation
+ if peak_mag < peak_mag_threshold
+ fprintf('bad match marker (%0.1f,%0.2f) in view %d\n', x_grid(ix), y_grid(iy), other_view_idx);
+ marker_shift= [0 0];
+ b_marker_visible(ix, iy, iv) = false;
+ else
+ marker_shift= (peak_loc(:)' - size(first_view_img)) .* [dx dy];
+ end
+
+ marker_shift_x(ix, iy, other_view_idx) = -marker_shift(1);
+ marker_shift_y(ix, iy, other_view_idx) = -marker_shift(2);
+ end
+
+ % update which views share an image of this marker
+ i_shared_views = find(b_marker_visible(ix, iy, :));
+
+ %% average out shifts so no image is preferred
+ average_shift_x = mean(marker_shift_x(ix, iy, i_shared_views));
+ average_shift_y = mean(marker_shift_y(ix, iy, i_shared_views));
+ marker_shift_x(ix, iy, i_shared_views) = marker_shift_x(ix, iy, i_shared_views) - average_shift_x;
+ marker_shift_y(ix, iy, i_shared_views) = marker_shift_y(ix, iy, i_shared_views) - average_shift_y;
+ end
+end
+
+fprintf('done\n');
+
+%% re-calibrate and re-dewarp
+
+fprintf('Recomputing calibration ... \n');
+
+rewarped_image_mat = dewarped_image_mat;
+
+for calib_idx = 1 : n_calibrations
+ %% report
+ fprintf(' - calibration %2d of %2d ... ', calib_idx, n_calibrations);
+
+ %% find markers on this image
+ b_visible_markers = b_marker_visible(:, :, calib_idx);
+ marker_x = x_grid_mat + marker_shift_x(:, :, calib_idx);
+ marker_y = y_grid_mat + marker_shift_y(:, :, calib_idx);
+ marker_x_old = marker_x(b_visible_markers);
+ marker_y_old = marker_y(b_visible_markers);
+ marker_x_new = x_grid_mat(b_visible_markers);
+ marker_y_new = y_grid_mat(b_visible_markers);
+ n_visible_markers = sum(b_visible_markers(:));
+
+ %% get updated i/j coordinates of marker positions
+ file_idx = calib_file_range(calib_idx);
+ file_name = sprintf([calibration_dir '/' calib_infile_pat], file_idx);
+ fstruct = importdata(file_name);
+ [marker_i,marker_j] = pinholemap_2d(marker_x_old(:), marker_y_old(:), fstruct.P);
+
+ %% recalibrate
+ marker_ij_tilde = [marker_i(:) marker_j(:) ones(n_visible_markers,1)]';
+ marker_xy_tilde = [marker_x_new(:) marker_y_new(:) ones(n_visible_markers,1)]';
+ P2D_new = dlt33(marker_ij_tilde, marker_xy_tilde);
+
+ %% re-dewarp image
+ im = fstruct.source_image;
+ i_axis = fstruct.i_axis;
+ j_axis = fstruct.j_axis;
+
+ [imat,jmat] = pinholemap_2d(xmat, ymat, P2D_new);
+ rewarped_img = interp2(i_axis, j_axis, im', imat, jmat, 'linear', nan);
+ rewarped_image_mat(calib_idx, :, :) = rewarped_img;
+
+ %% save
+ fstruct.P = P2D_new;
+ fstruct.ij_markers = [marker_i(:) marker_j(:)]';
+ fstruct.xy_markers = [marker_x_new(:) marker_y_new(:)]';
+ fstruct.x_axis = x;
+ fstruct.y_axis = y;
+ fstruct.dewarped_image = rewarped_img;
+
+ output_file = sprintf([calibration_dir '/' calib_outfile_pat], file_idx);
+ save(output_file, '-struct', 'fstruct');
+
+ fprintf('done\n');
+end
+
+fprintf('Done\n');
+
+%% compare dewarped and re-dewarped images
+
+for img_idx = 1 : n_calibrations
+ discrepancy_x = squeeze( marker_shift_x(:,:, img_idx) );
+ discrepancy_y = squeeze( marker_shift_y(:,:, img_idx) );
+ marker_x = x_grid_mat + discrepancy_x;
+ marker_y = y_grid_mat + discrepancy_y;
+ dewarped_img = squeeze(dewarped_image_mat(img_idx, :, :));
+ rewarped_img = squeeze(rewarped_image_mat(img_idx, :, :));
+
+ figure(2);
+ %{
+ subplot(1, 2, 1);
+ hold off;
+ imagesc(x, y, dewarped_img');
+ hold on;
+ plot(marker_x(:), marker_y(:), 'rx');
+ axis xy equal tight;
+ grid on;
+ xlim(x_lim);
+ ylim(y_lim);
+ %xlim([-25 25]);
+ %ylim([-25 25]);
+ title(sprintf('dewarped %d', img_idx));
+
+
+ subplot(1, 2, 2);
+ %}
+ hold off;
+ imagesc(x, y, rewarped_img');
+ colormap jet;
+ grid on;
+ axis xy equal tight;
+ xlim(x_lim);
+ ylim(y_lim);
+ title(sprintf('rewarped %d', img_idx));
+
+ pause();
+end
diff --git a/calibration/cubicfit_2d.m b/calibration/cubicfit_2d.m
new file mode 100644
index 0000000000000000000000000000000000000000..bd24206443094e13607a1b7c4918f034ab7dd87c
--- /dev/null
+++ b/calibration/cubicfit_2d.m
@@ -0,0 +1,64 @@
+function C = cubicfit_2d(i, j, X, Niter)
+ % fits coefficients C to observations of X(i,j) to create a model of X
+ % according to
+ % X* = C(1) + C(2)*i + C(3)*i.^2 + C(4)*i.^3 + ...
+ % + C(5)*j + C(6)*j.^2 + C(7)*j.^3 + ...
+ % + C(8)*i.*j + C(9)*i.^2.*j + C(10)*i.*j.^2;
+ %{
+ ii = i.^2;
+ iii = i.^3;
+ jj = j.^2;
+ jjj = j.^3;
+ ij = i.*j;
+ iij = i.^2.*i;
+ ijj = i.*j.^2;
+ Np = numel(i);
+ %}
+ myfun = @(X,xdata) cubicmap_2d(xdata(:,1), xdata(:,2), X);
+ C0 = zeros(1,10);
+ opts = optimset('MaxFunEvals', 100000, 'MaxIter', 5000, 'Display', 'off');
+ [C,~,res] = lsqcurvefit(myfun, C0, [i(:) j(:)], X(:), [], [], opts);
+ if nargin > 3
+ for iter = 2 : Niter
+ % cut out the chaff
+ thresh = prctile(res, 97);
+ ok = res < thresh;
+ i = i(ok);
+ j = j(ok);
+ X = X(ok);
+ % repeat
+ [C,~,res]= lsqcurvefit(myfun, C0, [i(:) j(:)], X(:), [], [], opts);
+ end
+ end
+
+
+ %{
+ A = [ones(Np,1) i(:) ii(:) iii(:) j(:) jj(:) jjj(:) ij(:) iij(:) ijj(:)];
+ b = X(:);
+ [C,~,res] = lsqlin(A, b, [], []);
+
+ % iteratively remove outliers
+ if nargin > 3
+ for iter = 2 : Niter
+ % cut out the chaff
+ thresh = prctile(res, 97);
+ ok = res < thresh;
+ i = i(ok);
+ ii = ii(ok);
+ iii = iii(ok);
+ j = j(ok);
+ jj = jj(ok);
+ jjj = jjj(ok);
+ ij = ij(ok);
+ iij = iij(ok);
+ ijj = ijj(ok);
+ X = X(ok);
+ Np = numel(i);
+ % repeat
+ A = [ones(Np,1) i(:) ii(:) iii(:) j(:) jj(:) jjj(:) ij(:) iij(:) ijj(:)];
+ b = X(:);
+ [C,~,res]= lsqlin(A, b, [], []);
+ end
+ end
+ %}
+end
\ No newline at end of file
diff --git a/calibration/cubicmap_2d.m b/calibration/cubicmap_2d.m
new file mode 100644
index 0000000000000000000000000000000000000000..77edbde8930e1dbdbee846015b4bea5d4f40bf9d
--- /dev/null
+++ b/calibration/cubicmap_2d.m
@@ -0,0 +1,7 @@
+function X = cubicmap_2d(i, j, C)
+ % maps [i,j] onto X = X(i,j) using coefficients provided in C
+ % coefficients derived from cubicfit_2d
+ X = C(1) + C(2)*i + C(3)*i.^2 + C(4)*i.^3 + ...
+ + C(5)*j + C(6)*j.^2 + C(7)*j.^3 + ...
+ + C(8)*i.*j + C(9)*i.^2.*j + C(10)*i.*j.^2;
+end
\ No newline at end of file
diff --git a/calibration/dewarp_2d.m b/calibration/dewarp_2d.m
new file mode 100644
index 0000000000000000000000000000000000000000..184366fe54b25a87506292516194dcd34fc7a92e
--- /dev/null
+++ b/calibration/dewarp_2d.m
@@ -0,0 +1,23 @@
+function dewarped_img = dewarp_2d(raw_img, xy_to_i, xy_to_j, x, y)
+ % reinterpolates raw_img onto grid [x,y] from [i,j] using the dewarping coefficients
+ % xy_to_i and xy_to_j
+
+ Nx = length(x);
+ Ny = length(y);
+ [xmat, ymat] = ndgrid(x, y);
+ imat = cubicmap_2d(xmat, ymat, xy_to_i);
+ jmat = cubicmap_2d(xmat, ymat, xy_to_j);
+
+ ivec = 1 : size(raw_img, 1);
+ jvec = 1 : size(raw_img, 2);
+ dewarped_img = interp2(ivec, jvec, double(raw_img'), imat, jmat, 'linear', 0);
+
+ % lanczos interpolation is faster than matlab's interp2 and better
+ % quality. kernel size is specified by ksize. 18 is good.
+ %{
+ ksize = 10;
+ xi = [imat(:)-1, jmat(:)-1]';
+ dewarped_img = interp2lanczos(single(raw_img), single(xi), ksize);
+ dewarped_img = reshape(dewarped_img, Nx, Ny);
+ %}
+end
\ No newline at end of file
diff --git a/calibration/dewarp_pinhole_2d.m b/calibration/dewarp_pinhole_2d.m
new file mode 100644
index 0000000000000000000000000000000000000000..f521baa05ebd824d7db396b454b098321b3beed6
--- /dev/null
+++ b/calibration/dewarp_pinhole_2d.m
@@ -0,0 +1,13 @@
+function im_uv = dewarp_pinhole_2d(im, P2D, u_axis, v_axis)
+ Nu = length(u_axis);
+ Nv = length(v_axis);
+ [umat, vmat] = ndgrid(u_axis, v_axis);
+ uv_tilde = [umat(:) vmat(:) ones(Nu*Nv,1)]';
+ ij_tilde = P2D * uv_tilde;
+ lambda = ij_tilde(3, :);
+ imat = reshape(ij_tilde(1,:) ./ lambda, [Nu Nv]);
+ jmat = reshape(ij_tilde(2,:) ./ lambda, [Nu Nv]);
+ i_axis = 1 : size(im, 1);
+ j_axis = 1 : size(im, 2);
+ im_uv = interp2(i_axis, j_axis, im', imat, jmat, 'linear');
+end
\ No newline at end of file
diff --git a/calibration/distortion_test.m b/calibration/distortion_test.m
new file mode 100644
index 0000000000000000000000000000000000000000..af101713f9992cfe9810492e5a1ca9a03e7f04ba
--- /dev/null
+++ b/calibration/distortion_test.m
@@ -0,0 +1,158 @@
+%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
+% fit calibration data obtained from pinhole camera model fit
+% to a higher order camera model
+%
+% we follow Machioane et al. 2016 to extract a transfer function that
+% maps image space coordinates to rays through object space
+%
+% 1) obtain 2D transfer function for each calibration plane
+% 2) trace rays back for each pixel, based on intersection
+% with each calibration plane
+% 3) fit a transfer function to map rays to
+
+addpath([pwd '/pinhole']);
+addpath([pwd '/poly']);
+
+%% input parameters
+
+calib_dir = '/nfs/data/lawson/160715/calib/';
+cam_calib_file_pat = [calib_dir '/CAM%d-%02d.mat'];
+cam_model_file = [calib_dir '/camera-model-original.mat'];
+
+cam_index_range = [1 2 3];
+cam_view_range = 1 : 16;
+n_views = length(cam_view_range);
+n_cameras = length(cam_index_range);
+n_iterations = 2;
+
+DO_DEBUG_PLOT = false;
+
+%% load model
+setup = importdata(camera_model_file);
+
+% camera calibration
+n_cameras = setup.n_cameras;
+n_views = setup.n_views;
+
+% plate pose
+gl_plate_pos = setup.gl_plate_pos;
+pl_to_gl_mat = repmat(setup.pl_to_gl, [1 1 n_views]);
+camera_model = setup.camera;
+gl_to_re = eye(3);
+gl_origin = setup.gl_origin;
+r_sphere = min(setup.r_sphere);
+n_views = setup.n_views;
+
+%% obtain 2D transfer function for each plane
+fprintf('Fitting 2D transfer function for each plate in each camera\n');
+fprintf('This gives best case scenario for how advanced model will reduce error\n');
+
+quiver_scale = 10;
+for c = 1 : n_cameras
+ %% initialise
+ camera = setup.camera(c);
+ i_axis = camera.i_axis;
+ j_axis = camera.j_axis;
+ ij_res_poly = [];
+ ij_res_pin = [];
+
+ gl_mark_ary = [];
+ ij_mark_ary = [];
+ sh_mark_ary = [];
+
+ %% loop over views
+ for v = 1 : n_views
+ %% get plate, image and world coordinates of markers
+ pl_to_gl = pl_to_gl_mat(:,:,v);
+ gl_pos = gl_plate_pos(:,v);
+ uv_mark = camera.view(v).uv_markers;
+ n_mark = size(uv_mark, 2);
+ gl_mark = bsxfun(@plus, gl_pos, pl_to_gl*[uv_mark; zeros(1,n_mark)]);
+ ij_mark = camera.view(v).ij_markers;
+
+ %% get sphere coordinates of markers
+ R = camera.calib.R;
+ xc = camera.calib.xc;
+ eu_mark = R * bsxfun(@minus, gl_mark, xc);
+ eu_len = sqrt(sum(eu_mark.^2,1));
+ eu_mark = bsxfun(@rdivide, eu_mark, eu_len);
+ theta_mark = acos(eu_mark(3,:));
+ phi_mark = atan2(eu_mark(2,:), eu_mark(1,:));
+
+ sh_mark = [theta_mark; phi_mark];
+
+ ij_mark_ary = [ij_mark_ary, ij_mark];
+ gl_mark_ary = [gl_mark_ary, gl_mark];
+ sh_mark_ary = [sh_mark_ary, sh_mark];
+
+ %% create transfer function
+ P_i2p = polyfit2d3(ij_mark, uv_mark);
+ P_p2i = polyfit2d3(uv_mark, ij_mark);
+ camera.view(v).P_i2p = P_i2p;
+ camera.view(v).P_p2i = P_p2i;
+
+ %% compare transfer function to pinhole model
+ uv_res = polymap2d3(P_i2p, ij_mark) - uv_mark;
+ ij_res_poly_view = polymap2d3(P_p2i, uv_mark) - ij_mark;
+ ij_res_pin_view = pinhole_model(gl_mark, camera.calib, true) - ij_mark;
+
+ ij_res_poly = [ij_res_poly, ij_res_poly_view];
+ ij_res_pin = [ij_res_pin, ij_res_pin_view];
+
+ %% debug plot
+ if DO_DEBUG_PLOT
+ %% residuals
+ figure(1);
+ clf;
+ quiversc(ij_mark(1,:), ij_mark(2,:), ...
+ ij_res_poly_view(1,:), ij_res_poly_view(2,:), quiver_scale, 'k-');
+ hold on;
+ quiversc(ij_mark(1,:), ij_mark(2,:), ...
+ ij_res_pin_view(1,:), ij_res_pin_view(2,:), quiver_scale, 'r-');
+ axis equal xy;
+ xlim(i_axis([1 end]));
+ ylim(j_axis([1 end]));
+ %%
+ pause();
+ end
+ end
+
+ %% polynomial map, undistorted image -> image
+ ij_proj_ary = pinhole_model(gl_mark_ary, camera.calib, false);
+ ij_res_ary = ij_proj_ary - ij_mark_ary;
+ P_u2d = polyfit2d4(ij_proj_ary, ij_mark_ary);
+ P_d2u = polyfit2d4(ij_mark_ary, ij_proj_ary);
+
+ ij_dist_ary = polymap2d4(P_u2d, ij_proj_ary);
+ ij_res_pp = ij_dist_ary - ij_mark_ary;
+
+ %% polynomial map, spherical -> image
+ P_s2i = polyfit2d3(sh_mark_ary, ij_mark_ary);
+ P_i2s = polyfit2d3(ij_mark_ary, sh_mark_ary);
+ camera.calib.P_s2i = P_s2i;
+ camera.calib.P_i2s = P_i2s;
+
+ %% mapping
+ figure(2);
+ quiversc(ij_mark_ary(1,:), ij_mark_ary(2,:), ...
+ ij_res_pp(1,:), ij_res_pp(2,:), 2, 'k-');
+ axis equal tight xy;
+ xlim(i_axis([1 end]));
+ ylim(j_axis([1 end]));
+
+ %% save
+ setup.camera(c) = camera;
+
+ %% report differences
+ ij_rms_poly = rms(ij_res_poly, 2);
+ ij_rms_pin = rms(ij_res_pin, 2);
+ ij_rms_pp = rms(ij_res_pp, 2);
+ ij_max_poly = max(abs(ij_res_poly), [], 2);
+ ij_max_pin = max(abs(ij_res_pin ), [], 2);
+ ij_max_pp = max(abs(ij_res_pp ), [], 2);
+ fprintf('Residuals in camera %d\n', c);
+ fprintf(' polynomial: %0.3f %0.3f px rms %0.3f %0.3f max\n', ij_rms_poly, ij_max_poly);
+ fprintf(' pinhole+dist: %0.3f %0.3f px rms %0.3f %0.3f max\n', ij_rms_pp , ij_max_pp);
+ fprintf(' pinhole+rad: %0.3f %0.3f px rms %0.3f %0.3f max\n', ij_rms_pin , ij_max_pin);
+end
+
diff --git a/calibration/dlt/dlt_maths.m b/calibration/dlt/dlt_maths.m
new file mode 100644
index 0000000000000000000000000000000000000000..8d89c2773ed66f693ade85f98f62bec0bf069c0f
--- /dev/null
+++ b/calibration/dlt/dlt_maths.m
@@ -0,0 +1,59 @@
+%% work out maths for parallel DLT
+% we have a problem where
+% y1_k ~ T1 * x_k
+% y2_k ~ T2 * x_k
+% but we don't know x_k, or T1 or T2
+%
+% so this can be written as
+% a_k * y1_k = T1 * x_k
+% b_k * y2_k = T2 * x_k
+% for k = 1 .. N equations
+%
+% if we choose an antisymmetric matrix Hi, then
+% a_k * y1_k' * Hi * y2_k = y1_k' * Hi * T1 * x_k
+% = 0
+% b_k * y2_k' * Hj * y2_k = y2_k' * Hj * T2 * x_k
+% = 0
+
+%% set up T1 and T2
+
+% we need two random orthogonal matrices
+Omega1 = randortho(3);
+Omega2 = randortho(3);
+% and random displacements xc1 and xc2
+xc1 = randn(3,1) * 100 + [0 1000 0]';
+xc2 = randn(3,1) * 100 + [1000 0 0]';
+T1 = Omega1 * [eye(3) -xc1];
+T2 = Omega2 * [eye(3) -xc2];
+T = [T1; T2];
+
+%% set up observations of xk, y1k and y2k
+
+N = 1000;
+xk = randn(3,N) * 100;
+xk_tilde = [xk; ones(1,N)];
+y1k = T1 * xk;
+y2k = T2 * xk;
+zk = [y1k; y2k];
+
+%% generate initial guess of xk, Omega1/2, xc1 and xc2
+
+xk_guess = xk;
+SNR = 100;
+for k = 1 : N
+ xk_guess(:,k) = xk(:,k) + randn(3,1)*norm(xk(:,k))/SNR;
+end
+
+omega1 = vrrotmat2vec(Omega1);
+omega2 = vrrotmat2vec(Omega2);
+omega1 = omega1(1:3) * omega1(4);
+omega2 = omega2(1:3) * omega2(4);
+omega1_guess= omega1 + randn(3,1) * norm(omega1) / SNR;
+omega2_guess= omega2 + randn(3,1) * norm(omega2) / SNR;
+xc1_guess = xc1 + norm(xc1)*randn(3,1)/SNR;
+xc2_guess = xc2 + norm(xc2)*randn(3,1)/SNR;
+
+
+
+%% generate initial guess for model
+
diff --git a/calibration/dlt/dlt_model.m b/calibration/dlt/dlt_model.m
new file mode 100644
index 0000000000000000000000000000000000000000..21053d0c8b90f4f4193932f3218921c262951fa4
--- /dev/null
+++ b/calibration/dlt/dlt_model.m
@@ -0,0 +1,40 @@
+function residuals = dlt_model(params, y1k, y2k, xc1, xc2)
+ %% extract model parameters
+ omega1 = params(1:3);
+ omega2 = params(4:6);
+ delta = [ 0 0 0]';%params(7:9);
+ xc1_prime= xc1(:) + delta(:);
+ xc2_prime= xc2(:) + delta(:);
+
+ % get rotation matrices
+ theta1 = norm(omega1);
+ theta2 = norm(omega2);
+ omega1 = [omega1/theta1; theta1];
+ omega2 = [omega2/theta2; theta2];
+ if theta1 < 1E-6; omega1 = [1 0 0 0]; end
+ if theta2 < 1E-6; omega2 = [1 0 0 0]; end
+ Omega1 = vrrotvec2mat(omega1);
+ Omega2 = vrrotvec2mat(omega2);
+
+ % get T1 and T2
+ T1 = Omega1 * [eye(3), -xc1_prime];
+ T2 = Omega2 * [eye(3), -xc2_prime];
+
+ % get model of xk
+ N = (length(params) - 6) / 3;
+ xk = reshape(params(7:end), [3 N]);
+
+ %% produce model result
+ H1 = [0 0 0; 0 0 -1; 0 1 0];
+ H2 = [0 0 1; 0 0 0; -1 0 0];
+ H3 = [0 -1 0; 1 0 0; 0 0 0];
+ residuals= zeros(6*N, 1);
+ for k = 1 : N
+ residuals((k-1)*6 + 1) = y1k(:,k)' * H1 * T1 * [xk(:,k); 1];
+ residuals((k-1)*6 + 2) = y1k(:,k)' * H2 * T1 * [xk(:,k); 1];
+ residuals((k-1)*6 + 3) = y1k(:,k)' * H3 * T1 * [xk(:,k); 1];
+ residuals((k-1)*6 + 4) = y2k(:,k)' * H1 * T2 * [xk(:,k); 1];
+ residuals((k-1)*6 + 5) = y2k(:,k)' * H2 * T2 * [xk(:,k); 1];
+ residuals((k-1)*6 + 6) = y2k(:,k)' * H3 * T2 * [xk(:,k); 1];
+ end
+end
\ No newline at end of file
diff --git a/calibration/dlt/dlt_test.m b/calibration/dlt/dlt_test.m
new file mode 100644
index 0000000000000000000000000000000000000000..dad8d0236c5a471803b3005d64e59ce212daa861
--- /dev/null
+++ b/calibration/dlt/dlt_test.m
@@ -0,0 +1,157 @@
+%% test solution of parallel DLT
+% we have a problem where
+% y1_k ~ T1 * x_k
+% y2_k ~ T2 * x_k
+% but we don't know x_k, or T1 or T2
+%
+% so this can be written as
+% a_k * y1_k = T1 * x_k
+% b_k * y2_k = T2 * x_k
+% for k = 1 .. N equations
+%
+% if we choose an antisymmetric matrix Hi, then
+% a_k * y1_k' * Hi * y2_k = y1_k' * Hi * T1 * x_k
+% = 0
+% b_k * y2_k' * Hj * y2_k = y2_k' * Hj * T2 * x_k
+% = 0
+%
+% solve using a non-linear solver
+% to find x_k, T1 and T2
+clear;
+clc;
+
+%% set up T1 and T2
+
+% we need two random orthogonal matrices
+Omega1 = randortho(3);
+Omega2 = randortho(3);
+% and random displacements xc1 and xc2
+xc1 = randn(3,1) * 100 + [0 1000 0]';
+xc2 = randn(3,1) * 100 + [1000 0 0]';
+delta = [0 0 0]';%randn(3,1) * 10;
+xc1_prime = xc1 + delta;
+xc2_prime = xc2 + delta;
+T1 = Omega1 * [eye(3), -xc1_prime];
+T2 = Omega2 * [eye(3), -xc2_prime];
+T = [T1; T2];
+
+%% set up observations of xk, y1k and y2k
+
+N = 100;
+xk = randn(3,N) * 100;
+xk_tilde = [xk; ones(1,N)];
+y1k = T1 * xk_tilde;
+y2k = T2 * xk_tilde;
+% apply arbitrary scaling a_k and b_k
+y1k_norm = y1k;
+y2k_norm = y2k;
+for k = 1 : N
+ y1k_norm(:,k) = y1k(:,k) / norm(y1k(:,k));
+ y2k_norm(:,k) = y2k(:,k) / norm(y2k(:,k));
+end
+
+zk = [y1k; y2k];
+
+%% generate initial guess of xk, Omega1/2, xc1 and xc2
+
+xk_guess = xk;
+SNR = 100;
+for k = 1 : N
+ xk_guess(:,k) = xk(:,k) + randn(3,1)*norm(xk(:,k))/SNR;
+end
+
+omega1 = vrrotmat2vec(Omega1);
+omega2 = vrrotmat2vec(Omega2);
+omega1 = omega1(1:3)' * omega1(4);
+omega2 = omega2(1:3)' * omega2(4);
+omega1_guess= omega1 + randn(3,1) * norm(omega1) / SNR;
+omega2_guess= omega2 + randn(3,1) * norm(omega2) / SNR;
+delta_guess = [0 0 0]';
+
+%% solve model
+
+X0 = [omega1_guess;
+ omega2_guess;
+ xk_guess(:)];
+fun = @(X) dlt_model(X, y1k, y2k, xc1, xc2);
+
+fprintf('Solving ... ');
+opts = optimset('TolFun', 1E-8, 'Display', 'on');
+[X_sol,n,res,flag] = lsqnonlin(fun, X0, [], [], opts);
+fprintf('done\n');
+fprintf('Solver exited with flag %d\n', flag);
+fprintf('RMS residual: %0.3e\n', sqrt(mean(res.^2)));
+fprintf('Norm of residuals: %0.3e\n', norm(res));
+
+%% extract solution
+
+omega1_sol = X_sol(1:3);
+omega2_sol = X_sol(4:6);
+delta_sol = [0 0 0]';
+xk_sol = reshape(X_sol(7:end), [3 N]);
+
+Omega1_sol = vrrotvec2mat([omega1_sol/norm(omega1_sol); norm(omega1_sol)]);
+Omega2_sol = vrrotvec2mat([omega2_sol/norm(omega2_sol); norm(omega2_sol)]);
+
+fprintf('|omega1 error| / |omega1| %0.3f %%\n', norm(omega1_sol-omega1)/norm(omega1)*100);
+fprintf('|omega2 error| / |omega2| %0.3f %%\n', norm(omega2_sol-omega2)/norm(omega2)*100);
+fprintf('|delta error| / |delta| %0.3f %%\n', norm(delta_sol-delta)/norm(delta)*100);
+
+figure(1);
+hold off;
+plot3(xk(1,:), xk(2,:), xk(3,:), 'b.');
+hold on;
+plot3(xk_sol(1,:), xk_sol(2,:), xk_sol(3,:), 'ro');
+axis equal
+title('Points in object space and their model solutions');
+
+%% check back-projected points line up
+T1_sol = Omega1_sol * [eye(3), -(xc1+delta_sol)];
+T2_sol = Omega2_sol * [eye(3), -(xc2+delta_sol)];
+y1k_sol = T1_sol * [xk_sol; ones(1,N)];
+y2k_sol = T2_sol * [xk_sol; ones(1,N)];
+% normalise
+for k = 1 : N
+ y1k_sol(:,k) = y1k_sol(:,k) / norm(y1k_sol(:,k));
+ y2k_sol(:,k) = y2k_sol(:,k) / norm(y2k_sol(:,k));
+end
+
+% compare unit vectors
+figure(2);
+hold off;
+plot3(y1k_norm(1,:), y1k_norm(2,:), y1k_norm(3,:), 'b.');
+hold on;
+plot3(y1k_sol(1,:) , y1k_sol(2,:) , y1k_sol(3,:) , 'bo');
+
+plot3(y2k_norm(1,:), y2k_norm(2,:), y2k_norm(3,:), 'r.');
+plot3(y2k_sol(1,:) , y2k_sol(2,:) , y2k_sol(3,:) , 'ro');
+
+xlim([-1 1]);
+ylim([-1 1]);
+zlim([-1 1]);
+%{
+
+%% check for rotation and translation
+
+% find best fit xk_sol = M * xk + x0
+% then find nearest orthogonal M
+M1 = lsqlin([xk' ones(N,1)], xk_sol(1,:)', [], []);
+M2 = lsqlin([xk' ones(N,1)], xk_sol(2,:)', [], []);
+M3 = lsqlin([xk' ones(N,1)], xk_sol(3,:)', [], []);
+M = [M1(1:3)'; M2(1:3)'; M3(1:3)'];
+d = [M1(4) M2(4) M3(4)]';
+
+figure(2);
+%[U,D,V] = svd(M);
+%M = U*V';
+M_xk = M * xk;
+M_xk(1,:)= M_xk(1,:) + M1(4);
+M_xk(2,:)= M_xk(2,:) + M2(4);
+M_xk(3,:)= M_xk(3,:) + M3(4);
+
+figure(2);
+hold off;
+plot3(M_xk(1,:), M_xk(2,:), M_xk(3,:), 'b.');
+hold on;
+plot3(xk_sol(1,:), xk_sol(2,:), xk_sol(3,:), 'r.');
+%}
\ No newline at end of file
diff --git a/calibration/dlt/randomorthogonal.m b/calibration/dlt/randomorthogonal.m
new file mode 100644
index 0000000000000000000000000000000000000000..cfb5359fff4714fb14e0c80bd8d349d068b2ca81
--- /dev/null
+++ b/calibration/dlt/randomorthogonal.m
@@ -0,0 +1,5 @@
+function Q = randomorthogonal(N)
+ S = randn(N);
+ [Q,D] = eig(S+S');
+ Q = Q * det(Q);
+end
\ No newline at end of file
diff --git a/calibration/dlt/randortho.m b/calibration/dlt/randortho.m
new file mode 100644
index 0000000000000000000000000000000000000000..a31c3d0451d9c60b2352db1f9b74c0b95f94f91e
--- /dev/null
+++ b/calibration/dlt/randortho.m
@@ -0,0 +1,6 @@
+function Q = randortho(N)
+ % create a random, orthogonal NxN matrix
+ S = randn(N);
+ [Q,D] = eig(S+S');
+ Q = Q * det(Q);
+end
\ No newline at end of file
diff --git a/calibration/find_calibration_markers.m b/calibration/find_calibration_markers.m
new file mode 100644
index 0000000000000000000000000000000000000000..de47e973454f78df82653cebc6282d4fa4b1e3eb
--- /dev/null
+++ b/calibration/find_calibration_markers.m
@@ -0,0 +1,195 @@
+function [ij_markers, xy_markers, on_x_axis, on_y_axis] = find_calibration_markers(img, ij_center, ij_right, ij_top, dot_half_size)
+ % find_markers(img, ij_center, ij_top, ij_right)
+ %
+ % find markers on a regular calibration grid in the image img
+ % x_markers is returned, which contains the [i,j] coordinates (in image
+ % space) of each marker identified. in addition, also return the [x,y]
+ % coordinates of each marker. these are, by definition, integer values
+ %
+ % to start the search, the user must provide the [i,j] coordinates of
+ % ij_center - the calibration marker at the center of the coordinate
+ % system
+ % ij_top - the calibration marker immediately above the center marker
+ % ij_right - the calibration marker immediately to the right of the
+ % center marker
+
+ %% put data in right format
+ ij_center = ij_center(:);
+ ij_top = ij_top(:);
+ ij_right = ij_right(:);
+
+ %% get size of image
+ Nx = size(img, 1);
+ Ny = size(img, 2);
+
+ thresh = 0.6; % minimum correlation coefficient required for match
+
+ %% get dot img
+ dot_size = dot_half_size * 2 + 1;
+ dot_img_i = round(ij_center(1)) + (-dot_half_size(1) : dot_half_size(1));
+ dot_img_j = round(ij_center(2)) + (-dot_half_size(2) : dot_half_size(2));
+ if dot_img_i(1) < 1 || dot_img_i(end) > Nx || dot_img_j(1) < 1 dot_img_j(end) > Ny
+ error('Center dot is too close to edge of image');
+ end
+ dot_img = img(dot_img_i, dot_img_j);
+
+ %% convolve image and find indices of maxima
+ dot_img = dot_img - mean(dot_img(:));
+ % subtract sliding average so DC component of x-correlation windows is
+ % zero
+ img_sliding_av = filter2(ones(size(dot_img)), img) / numel(dot_img);
+ img = img - img_sliding_av;
+ % cross-correlate
+ convolved = filter2(dot_img, img);
+ % normalise by image energy
+ image_energy = filter2(ones(size(dot_img)), img.^2);
+ dot_energy = sum(dot_img(:).^2);
+ correl = convolved ./ (sqrt(image_energy) * sqrt(dot_energy));
+ %correl = correl / correl(round(ij_center(1)), round(ij_center(2)));
+
+ correl_left = inf([Nx Ny]);
+ correl_right = inf([Nx Ny]);
+ correl_above = inf([Nx Ny]);
+ correl_below = inf([Nx Ny]);
+
+ correl_left(1:Nx-2, 2:Ny-1) = correl(2:Nx-1, 2:Ny-1);
+ correl_right(3:Nx, 2:Ny-1) = correl(2:Nx-1, 2:Ny-1);
+ correl_above(2:Nx-1, 1:Ny-2) = correl(2:Nx-1, 2:Ny-1);
+ correl_below(2:Nx-1, 3:Ny) = correl(2:Nx-1, 2:Ny-1);
+
+ bw_markers = correl > correl_left ...
+ & correl > correl_right ...
+ & correl > correl_below ...
+ & correl > correl_above ...
+ & correl > thresh;
+ %figure(2);
+ %imagesc(correl' > thresh);
+ %axis equal tight xy;
+ %% identify likely markers which are on the grid
+ ivec = 1 : Nx;
+ jvec = 1 : Ny;
+ [imat,jmat] = ndgrid(ivec, jvec);
+
+ Nmarkers = sum(bw_markers(:));
+ ij_markers = zeros(2, Nmarkers);
+ ij_markers(1,:)= imat(bw_markers);
+ ij_markers(2,:)= jmat(bw_markers);
+
+ % find the position of the top/right markers more accurately
+ ij_right = ij_markers(:, knnsearch(ij_markers', ij_right', 'K', 1));
+ ij_top = ij_markers(:, knnsearch(ij_markers', ij_top', 'K', 1));
+
+ % search for dots on the i/j axes
+ dx = norm(ij_center - ij_right);
+ dy = norm(ij_center - ij_top);
+ e_x = (ij_right - ij_center) / dx;
+ e_y = (ij_top - ij_center) / dy;
+ % angle tolerance, in radians, that must be satisfied in order for a
+ % point to be considered on the x or y axes. angle is taken between the
+ % x/y axis and the line between the center and the point under
+ % consideration
+ delta_theta_x = 0.5 * dy / Nx;
+ delta_theta_y = 0.5 * dx / Ny;
+
+ ij_rel_markers = ij_markers - ij_center * ones(1,Nmarkers);
+ ij_rel_length = sqrt(sum(ij_rel_markers.^2,1));
+ [~,center_idx] = min(ij_rel_length);
+ e_x_mat = e_x * ones(1,Nmarkers);
+ e_y_mat = e_y * ones(1,Nmarkers);
+ cos_theta_x = dot(ij_rel_markers, e_x_mat, 1) ./ ij_rel_length;
+ cos_theta_y = dot(ij_rel_markers, e_y_mat, 1) ./ ij_rel_length;
+
+ % get indices of points on x and y axes
+ % include the center marker in this
+ on_x_axis = find(abs(cos_theta_x) > cos(delta_theta_x));
+ on_y_axis = find(abs(cos_theta_y) > cos(delta_theta_y));
+ on_x_axis = sort(unique([on_x_axis, center_idx]));
+ on_y_axis = sort(unique([on_y_axis, center_idx]));
+ Nonx = length(on_x_axis);
+ Nony = length(on_y_axis);
+
+ %% eliminate markers which are not at the right spacing
+ % if distance is more than a percentage threshold out, reject
+ x_on_x = dot(ij_rel_markers(:,on_x_axis), e_x*ones(1,Nonx), 1);
+ y_on_y = dot(ij_rel_markers(:,on_y_axis), e_y*ones(1,Nony), 1);
+ [x_valid,x_ind]= find_equispaced_values(x_on_x, dx);
+ [y_valid,y_ind]= find_equispaced_values(y_on_y, dy);
+ % truncate markers which are not equispaced
+ on_x_axis = on_x_axis(x_valid);
+ on_y_axis = on_y_axis(y_valid);
+ Nonx = length(on_x_axis);
+ Nony = length(on_y_axis);
+ % identify which coordinate each marker corresponds to on x/y axes
+ [~,x_center] = min(abs(x_on_x(x_valid)));
+ [~,y_center] = min(abs(y_on_y(y_valid)));
+ x_axis = x_ind - x_ind(x_center);
+ y_axis = y_ind - y_ind(y_center);
+
+ %% improve estimate of e_x and e_y
+ px = polyfit(ij_markers(1,on_x_axis), ij_markers(2,on_x_axis), 1);
+ py = polyfit(ij_markers(2,on_y_axis), ij_markers(1,on_y_axis), 1);
+ e_x = [1; px(1)] / sqrt(1 + px(1)^2);
+ e_y = [py(1); 1] / sqrt(1 + py(1)^2);
+
+ %% identify markers which lie on the grid
+ b_on_grid = false(1, Nmarkers);
+ b_on_grid(center_idx)= true;
+ b_on_grid(on_x_axis) = true;
+ b_on_grid(on_y_axis) = true;
+ xy_markers = zeros(2, Nmarkers);
+ xy_markers(1,on_x_axis) = x_axis;
+ xy_markers(2,on_y_axis) = y_axis;
+ % for each point on x-axis, search up/down for points // to y axis
+ for xidx = 1 : Nonx
+ % identify markers on the axis with x = const, // to y axis
+ marker_idx = on_x_axis(xidx);
+ ij_rel_markers = ij_markers - ij_markers(:,marker_idx)*ones(1, Nmarkers);
+ ij_rel_length = sqrt(sum(ij_rel_markers.^2,1));
+ cos_theta_y = dot(ij_rel_markers, e_y_mat, 1) ./ ij_rel_length;
+ on_this_axis = abs(cos_theta_y) > cos(delta_theta_y);
+ b_on_grid = b_on_grid | on_this_axis;
+ % save the x/y coord of these markers
+ Nonthis = sum(on_this_axis);
+ y_component = dot(ij_rel_markers(:,on_this_axis), e_y*ones(1,Nonthis), 1);
+ xy_markers(1,on_this_axis) = x_axis(xidx);
+ xy_markers(2,on_this_axis) = round(y_component/dy);
+ end
+
+ %% delete markers are not on grid or accidentally have duplicates
+ b_on_x_axis = false(1, Nmarkers);
+ b_on_x_axis(on_x_axis) = true;
+ b_on_y_axis = false(1, Nmarkers);
+ b_on_y_axis(on_y_axis) = true;
+
+ ij_markers = ij_markers(:, b_on_grid);
+ xy_markers = xy_markers(:, b_on_grid);
+ b_on_x_axis = b_on_x_axis(b_on_grid);
+ b_on_y_axis = b_on_y_axis(b_on_grid);
+ % get rid of markers which are repeated more than once
+ [~,IA] = unique(xy_markers', 'rows');
+ repeats = setxor(IA, 1 : size(xy_markers,2));
+ [xy_markers,IA]= setxor(xy_markers', xy_markers(:, repeats)', 'rows');
+ xy_markers = xy_markers';
+ ij_markers = ij_markers(:, IA);
+ b_on_x_axis = b_on_x_axis(IA);
+ b_on_y_axis = b_on_y_axis(IA);
+ on_x_axis = find(b_on_x_axis);
+ on_y_axis = find(b_on_y_axis);
+ idx_center = find(ij_markers(1,:)==ij_center(1) & ij_markers(2,:)== ij_center(2));
+
+ %% plot things
+ %{
+ hold off;
+ imagesc(img'); axis xy;
+ hold on;
+ plot(ij_markers(1,:), ij_markers(2,:), 'r+');
+ axis equal tight;
+ colormap gray;
+ plot(ij_markers(1,on_x_axis), ij_markers(2,on_x_axis), 'go');
+ plot(ij_markers(1,on_y_axis), ij_markers(2,on_y_axis), 'gd');
+ plot(ij_markers(1,idx_center), ij_markers(2,idx_center), 'bs');
+ colormap gray;
+ %}
+end
+
+
diff --git a/calibration/find_equispaced_values.m b/calibration/find_equispaced_values.m
new file mode 100644
index 0000000000000000000000000000000000000000..22861dd663d00f12d092c0e6e42c24f3d37cee31
--- /dev/null
+++ b/calibration/find_equispaced_values.m
@@ -0,0 +1,135 @@
+function [equispaced, index] = find_equispaced_values(x, dx_guess)
+ % for a set of values x
+ % which are approximately equispaced, but with some invalid points added
+ % that are not equispaced, try to find the points which are equally
+ % spaced apart
+
+ %% sort x ascending
+ [x,idx] = sort(x, 'ascend');
+ [~,reverse] = sort(idx, 'ascend');
+ Nx = length(x);
+
+ %% identify potential collisions
+ % place values into bins which are a fraction of initial guess dx
+ % if values are repeated approximately dx apart, then there should only
+ % be one per bin
+
+ bin_width = 0.8 * dx_guess;
+ bin_index = round(x / bin_width);
+ bin_centres = bin_width * (min(bin_index) : max(bin_index));
+ n_bins = length(bin_centres);
+ n_in_bin = zeros(1, n_bins); % number of elements in this bin
+ idx_in_bin = cell(1, n_bins); % index of elements in x belonging to this bin
+ possible_repeat= false(1, Nx);
+ for i = 1 : n_bins
+ centre_distance= abs(x - bin_centres(i));
+ in_this_bin = centre_distance < bin_width / 2;
+ n_in_bin(i) = sum(in_this_bin);
+ idx_in_bin{i} = find(in_this_bin);
+ if n_in_bin(i) > 1
+ possible_repeat(in_this_bin) = true;
+ end
+ end
+
+ %% find typical spacing of values not likely to be collisions
+ dx_vec = x(2:end) - x(1:end-1);
+ dx_vec = dx_vec(:);
+ dx_vec = ([dx_vec; dx_vec(end)] + [dx_vec(1); dx_vec]) / 2; % mean of left and right spacing
+ x_filtered = x(~possible_repeat);
+ dx_filtered = dx_vec(~possible_repeat);
+ dx_meas = median(dx_filtered);
+
+ %% preliminary search
+ % filter the data into a non-contiguous set of bins
+ % which are spaced dx_meas apart, but are narrow
+ % and find the offset that maximises the number of items identified
+ %
+ % ----x----x----x----x---- 0 fall into bins
+ % ----x----x----x----x---- 0
+ % ----x----x----x----x---- 0
+ % ----x----x----x----x---- 1
+ % ----x----x----x----x---- 4
+ % * * * ** * *
+ bin_spacing = dx_meas;
+ bin_width = 0.15 * bin_spacing; % treated as half-width
+ bin_offset = (-dx_meas/2) : bin_width/4 : (dx_meas/2);
+ Nbins = length(bin_offset);
+ Nvalid = zeros(1, Nbins);
+ for iter = 1 : Nbins
+ x_offset = x_filtered - bin_offset(iter);
+ in_a_bin = abs(x_offset - round(x_offset/bin_spacing)*bin_spacing) < bin_width;
+ Nvalid(iter)= sum(in_a_bin(:));
+ end
+ [~,best] = max(Nvalid);
+ best_bin_offset= bin_offset(best);
+
+ % identify values classified as equispaced
+ % ignoring repeat values
+ x_offset = x - bin_offset(best);
+ bin_index = round(x_offset / bin_spacing);
+ bin_centres = bin_spacing * (min(bin_index) : max(bin_index));
+ n_bins = length(bin_centres);
+ n_in_bin = zeros(1, n_bins);
+ equispaced = false(1, Nx);
+ for i = 1 : n_bins
+ centre_distance= abs(x_offset - bin_centres(i));
+ [d,closest] = min(centre_distance);
+ if d < bin_width
+ equispaced(closest) = true;
+ end
+ end
+
+ % select only one contiguous block of equispaced points
+ [labelled,N] = bwlabeln(equispaced);
+ blocksize = zeros(1,N);
+ for i = 1 : N
+ blocksize(i) = sum(labelled == i);
+ end
+ [~,biggest_block] = max(blocksize);
+ equispaced = equispaced & labelled == biggest_block;
+
+ %% apply less conservative search
+ % first search is usually too conservative, because of error in dx_meas
+ % however, it gives us a good base for the next step
+ % starting from the left, decide if a point in x belongs in the
+ % equispaced set, because it's approximately dx_meas away from the last
+ % point in that set
+ bin_width = 0.15 * dx_meas;
+ for candidate = 2 : Nx
+ last_good = find(equispaced & ((1:Nx) < candidate), 1, 'last');
+ if isempty(last_good)
+ continue;
+ end
+ dx = x(candidate) - x(last_good);
+ if abs(dx - round(dx/dx_meas)*dx_meas) < bin_width && abs(round(dx/dx_meas)) >= 1
+ equispaced(candidate) = true;
+ end
+ end
+ % apply same search from right to left
+ for candidate = Nx-1 : -1 : 1
+ first_good = find(equispaced & ((1:Nx) > candidate), 1, 'first');
+ if isempty(first_good)
+ continue;
+ end
+ dx = x(first_good) - x(candidate);
+ if abs(dx - round(dx/dx_meas)*dx_meas) < bin_width && abs(round(dx/dx_meas)) >= 1
+ equispaced(candidate) = true;
+ end
+ end
+
+ %% index the markers
+ Nvalid = sum(equispaced(:));
+ i_valid = find(equispaced);
+ index = zeros(1,Nvalid);
+ for it = 2 : Nvalid
+ dx = x(i_valid(it)) - x(i_valid(it-1));
+ index(it) = index(it-1) + round(dx/dx_meas);
+ end
+ index_full = nan(1, Nx);
+ index_full(equispaced) = index;
+
+ %% undo the sorting
+ equispaced = equispaced(reverse);
+ index_full = index_full(reverse);
+ index = index_full(equispaced);
+end
\ No newline at end of file
diff --git a/calibration/find_intersection.m b/calibration/find_intersection.m
new file mode 100644
index 0000000000000000000000000000000000000000..9241f2c4e5eac2a56ffce9d79f270c061cc8180d
--- /dev/null
+++ b/calibration/find_intersection.m
@@ -0,0 +1,142 @@
+function [m, c, width, line_u, line_v, bSuccess] = find_intersection(u, v, sheet_img, max_thickness, v_min, v_max, grid_du, grid_dv)
+ % find_intersection(image)
+ %
+ % finds the line of intersection of a laser sheet and a calibration
+ % plate in the image provided, in the format provided by readimx
+ %
+ % the image should have been dewarped in Davis first
+ %
+ % returns the line of intersection u = m*v + c in the image coordinate
+ % system [u,v]
+ % as well as the e^-2 thickness w of the laser sheet, as it is observed when
+ % projected onto the calibration plate
+ %
+ % a gaussian profile of intensity for the laser sheet is assumed
+ %
+ % additionally, it is possible to specify:
+ % max_thickness - the maximum thickness the laser sheet should be
+ % v_min - minimum v to search for the intersection
+ % v_max - maximum v to search for the intersection
+
+ %% load coordinate system
+ search_width = 10;
+ grid_du = 10;
+ grid_dv = 10;
+ Nu = length(u);
+ Nv = length(v);
+
+ %% filter image
+ G = fspecial('gaussian', [7 7], 2);
+ sheet_img_filt = filter2(G, sheet_img);
+
+ %% identify points on sheet
+ % find maximum intensity along rows
+ ok_range = v > v_min & v < v_max;
+ line_v = v;
+ line_u = zeros(1, Nv);
+ bSuccess = false;
+ for j = 1:Nv
+ [m,i_maximum] = max(sheet_img_filt(:, j));
+ if m > 500
+ bSuccess = true;
+ end
+ line_u(j) = u(i_maximum);
+ end
+
+ if(~bSuccess)
+ m = nan();
+ c = nan();
+ width = nan();
+ return;
+ end
+
+ % find equation of fit
+ line_coeffs = polyfit(line_v(ok_range), line_u(ok_range), 1);
+ m = line_coeffs(1);
+ c = line_coeffs(2);
+
+ %% check whether fit is any good
+ % if it's not, don't bother continuing
+ fit_error = line_u(ok_range) - polyval(line_coeffs, line_v(ok_range));
+ std_error = sqrt(prctile(fit_error.^2,66));
+ max_error = abs(u(2) - u(1)) * 10;
+ if std_error > max_error
+ m = nan();
+ c = nan();
+ width = nan();
+ bSuccess = false;
+ return;
+ end
+ bSuccess = true;
+
+ %% improve fit and measure e^-2 width
+ % hack to avoid measurements across calibration markers
+ v_min = round(v_min/grid_dv) * grid_dv;
+ v_max = round(v_max/grid_dv) * grid_dv;
+ v_min = max(min(v), v_min);
+ v_max = min(max(v), v_max);
+ line_v = linspace(v_min, v_max, Nrows);
+ for row = 1 : Nrows
+ rem = line_v(row) - round(line_v(row)/grid_dv)*grid_dv;
+ if abs(rem) < 0.15 * grid_dv
+ if rem < 0
+ line_v(row) = (round(line_v(row)/grid_dv)-0.15) * grid_dv;
+ else
+ line_v(row) = (round(line_v(row)/grid_dv)+0.15) * grid_dv;
+ end
+ end
+ end
+ % v must be at integer pixel location
+ row_idx = round(interp1(v, 1 : Nv, line_v));
+ row_idx = min(max(row_idx,1),Nv);
+ line_v = v(row_idx);
+ line_u = zeros(1, Nrows);
+ line_width = zeros(1, Nrows);
+
+ for row = 1 : Nrows
+ j = row_idx(row);
+ u_line = m*v(j) + c;
+ nearby = abs(u' - u_line) < search_width/2;
+ dc_level = mean(sheet_img(:, j));
+ thresh = max(sheet_img(nearby,j) - dc_level) * exp(-2);
+ lb = find(nearby & (sheet_img(:,j)-dc_level) > thresh, 1, 'first');
+ ub = find(nearby & (sheet_img(:,j)-dc_level) > thresh, 1, 'last');
+ nearby = lb : ub;
+ u_nearby = u(nearby);
+ sig_nearby = sheet_img(nearby, j) + 1;
+
+ % fit a cubic polynomial,
+ % then around the maximum, evaluate coefficients for a quadratic
+ % polynomial in that vicinity.
+ %{
+ p = polyfit(u_nearby(:), log(sig_nearby(:)), 3);
+ p_prime = [3*p(1) 2*p(2) p(3)];
+ extrema = roots(p_prime);
+ if isreal(extrema)
+ % identify which extrema is the maximum, find taylor series
+ % expansion around it
+ p_2prime = [6*p(1) 2*p(2)];
+ extrema_type = polyval(p_2prime, extrema);
+ x0 = extrema(extrema_type < 0);
+ f_x0 = polyval(p, x0);
+ fp_x0 = polyval(p_prime, x0);
+ fpp_x0 = polyval(p_2prime, x0);
+ p = [(fpp_x0/2), (fp_x0 - fpp_x0*x0), (f_x0 - fp_x0*x0 + fpp_x0*x0^2/2)];
+ else
+ % the fit doesn't have a local maximum/minimum,
+ % i.e. we get imaginary roots. resort to a quadratic fit
+ p = polyfit(u_nearby(:), log(sig_nearby(:)), 2);
+ end
+ %}
+ p = polyfit(u_nearby(:), log(sig_nearby(:)), 2);
+
+ line_u(row) = -p(2) / (2*p(1));
+ line_width(row)= sqrt(-8 / p(1));
+ end
+
+ ok = ~imag(line_width) & ~isnan(line_width);
+ width = mean(line_width(ok));
+ line_coeffs = polyfit(line_v(ok), line_u(ok), 1);
+ m = line_coeffs(1);
+ c = line_coeffs(2);
+end
diff --git a/calibration/find_largest_volume.m b/calibration/find_largest_volume.m
new file mode 100644
index 0000000000000000000000000000000000000000..fc66240e80c693235d44d1d471de5562b8f5892f
--- /dev/null
+++ b/calibration/find_largest_volume.m
@@ -0,0 +1,50 @@
+function [XLim, YLim, ZLim] = find_largest_volume(P, ilim, jlim, M1, M2)
+ % for the projection matrix P
+ % which maps
+ % [i j 1]' ~ P[x y z 1]'
+ %
+ % find the largest volume
+ % xlim(1) <= x <= xlim(2)
+ % ylim(1) <= y <= ylim(2)
+ % zlim(1) <= z <= zlim(2)
+ %
+ % which is bound by planes represented by M1 and M2
+ % points on a plane satisfy M(1:3)*x + M(4) = 0
+ %
+ % which may be projected onto [i,j] and still fit within
+ % ilim(1) < i < ilim(2)
+ % jlim(1) < j < jlim(2)
+ %
+ %
+
+ % M1 and M2 are equ
+ P1 = [P; M1];
+ P2 = [P; M2];
+ P1i = P1^-1;
+ P2i = P2^-1;
+
+ % corners on image plane
+ i_corners = [ilim(1) ilim(1) ilim(2) ilim(2)];
+ j_corners = [jlim(1) jlim(2) jlim(2) jlim(1)];
+ ij10 = [i_corners(:) j_corners(:) ones(4,1) zeros(4,1)];
+
+ % corners on plane 1
+ lambda_inv = ij10 * P1i(4,:)';
+ x_corners = ij10 * P1i(1,:)' ./ lambda_inv;
+ y_corners = ij10 * P1i(2,:)' ./ lambda_inv;
+ z_corners = ij10 * P1i(3,:)' ./ lambda_inv;
+
+ % corners on plane 2
+ lambda_inv = ij10 * P2i(4,:)';
+ x_corners = [x_corners; ij10 * P2i(1,:)' ./ lambda_inv];
+ y_corners = [y_corners; ij10 * P2i(2,:)' ./ lambda_inv];
+ z_corners = [z_corners; ij10 * P2i(3,:)' ./ lambda_inv];
+
+ % use this simple trick to work out largest box that fits
+ XLim = sort(x_corners, 'ascend');
+ YLim = sort(y_corners, 'ascend');
+ ZLim = sort(z_corners, 'ascend');
+ XLim = XLim([4 5]);
+ YLim = YLim([4 5]);
+ ZLim = ZLim([4 5]);
+end
\ No newline at end of file
diff --git a/calibration/flip_calibration.m b/calibration/flip_calibration.m
new file mode 100644
index 0000000000000000000000000000000000000000..7f1ac81ee3d97963c3336caf0a554b38dde816d1
--- /dev/null
+++ b/calibration/flip_calibration.m
@@ -0,0 +1,30 @@
+%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
+% flip_calibration.m
+%
+% flip the axes of a camera calibration
+
+%% input parameters
+FLIP_X = false;
+FLIP_Y = true;
+calib_dir = '/nfs/data/lawson/iso60/160726/calib/';
+calib_file_in = [calib_dir 'camera-model-original.mat'];
+calib_file_out = [calib_dir 'camera-model-flip.mat'];
+
+%% load
+fprintf('Loading %s\n', calib_file_in);
+setup = importdata(calib_file_in);
+
+% quick fix of camera model
+for c = 1 : setup.n_cameras
+ if FLIP_X
+ setup.camera(c).i_axis = setup.camera(c).i_axis(end:-1:1);
+ end
+ if FLIP_Y
+ setup.camera(c).j_axis = setup.camera(c).j_axis(end:-1:1);
+ end
+end
+
+%% save
+fprintf('Saving %s\n', calib_file_out);
+save(calib_file_out, '-struct', 'setup');
+fprintf('done\n');
\ No newline at end of file
diff --git a/calibration/frustum.m b/calibration/frustum.m
new file mode 100644
index 0000000000000000000000000000000000000000..6fdccc9326be02e84b30eb1c9c8fa447bee60be7
--- /dev/null
+++ b/calibration/frustum.m
@@ -0,0 +1,60 @@
+function [M,U,xc] = frustum(cmodel)
+ % [M,U,xc] = frustum(cmodel)
+ %
+ % Calculate the viewing frustum for the pinhole camera model specified
+ % by cmodel
+ %
+ % M is a 4x4 matrix whose rows provide the equation of plane for each
+ % plane of the viewing frustum. That is to say, for each plane i,
+ % points satisfying:
+ % M(i,1)*x + M(i,2)*y + M(i,3)*z + M(i,4) = 0
+ % lie on that plane
+ %
+ % The plane normals are oriented so that (0,0,0) always corresponds to a
+ % positive distance
+ %
+ % U is a 3x4 matrix whose columns describe the unit vectors of lines
+ % describing the viewing frustum. Each edge i of the frustum is given by
+ % the equation of a line:
+ %
+ % x(s) = xc + u(:,i)*s
+
+ %% extract pinhole model parameters
+ i_axis = cmodel.i_axis;
+ j_axis = cmodel.j_axis;
+ ic = cmodel.calib.ic;
+ kr = cmodel.calib.kr;
+ G = cmodel.calib.G;
+ R = cmodel.calib.R;
+ GR = G*R;
+ xc = cmodel.calib.xc;
+
+ %% calculate corners
+ ij_corners = [i_axis([1 1 end end]);
+ j_axis([1 end end 1])];
+ % account for distortion
+ ij_corners = inverse_radial_distortion(ij_corners, ic, kr);
+
+ %% find unit vectors
+ ij_tilde = [ij_corners; ones(1,4)];
+ U = GR \ ij_tilde;
+ U_norm = sqrt(sum(U.^2,1));
+ U = U ./ ([1;1;1]*U_norm);
+ % orientation is such that u . xc < 0
+ flip = -sign(U.' * xc)';
+ U = U .* ([1;1;1]*flip);
+
+ %% find equations of plane
+ M = zeros(4,4);
+ for c = 1 : 4
+ m = cross(U(:,c), U(:,mod(c,4)+1));
+ m = m / norm(m);
+ d = -dot(m, xc);
+ % plane normals are oriented to ensure (0,0,0) is always on +ve side
+ if d<0
+ d = -d;
+ m = -m;
+ end
+ M(c,:) = [m', d];
+ end
+end
\ No newline at end of file
diff --git a/calibration/local_maxima2.m b/calibration/local_maxima2.m
new file mode 100644
index 0000000000000000000000000000000000000000..2f3b5e3beae3eee86503f3ce3b0783a11b7a0fec
--- /dev/null
+++ b/calibration/local_maxima2.m
@@ -0,0 +1,16 @@
+function locmin = local_maxima2(im)
+ sz = size(im);
+ rx = 2 : sz(1)-1;
+ ry = 2 : sz(2)-1;
+ locmin = false(sz);
+ locmin(rx,ry) = true;
+
+ for i = -1 : 1
+ for j = -1 : 1
+ if i == j
+ continue;
+ end
+ locmin(rx,ry) = locmin(rx,ry) & im(rx,ry) > im(rx+i,ry+j);
+ end
+ end
+end
\ No newline at end of file
diff --git a/calibration/local_maxima3.m b/calibration/local_maxima3.m
new file mode 100644
index 0000000000000000000000000000000000000000..16bebfa7f00c8a40bfb74f5e2c9280e45c0b871e
--- /dev/null
+++ b/calibration/local_maxima3.m
@@ -0,0 +1,22 @@
+function locmin = local_minima3(X)
+ % return bool array of points that are local minima in the array x
+ sz = size(X);
+ rx = 2 : sz(1) - 1;
+ ry = 2 : sz(2) - 1;
+ rz = 2 : sz(3) - 1;
+
+ locmin = true(sz-2);
+
+ for i = -1 : 1
+ for j = -1 : 1
+ for k = -1 : 1
+ if i == j && j == k
+ continue;
+ end
+
+ locmin = locmin & X(rx,ry,rz) > X(rx+i,ry+j,rz+k);
+ end
+ end
+ end
+ locmin = padarray(locmin, [1 1 1], false, 'both');
+end
diff --git a/calibration/local_minima.m b/calibration/local_minima.m
new file mode 100644
index 0000000000000000000000000000000000000000..cde435bb3d31ff1b271482f6e25171e493c276b5
--- /dev/null
+++ b/calibration/local_minima.m
@@ -0,0 +1,15 @@
+function locmin = local_minima(im)
+ sz = size(im);
+ locmin = true(sz);
+ rx = 2 : sz(1)-1;
+ ry = 2 : sz(2)-1;
+
+ for i = -1 : 1
+ for j = -1 : 1
+ if i == j
+ continue;
+ end
+ locmin(rx,ry) = locmin(rx,ry) & im(rx,ry) < im(rx+i,ry+j);
+ end
+ end
+end
\ No newline at end of file
diff --git a/calibration/local_minima3.m b/calibration/local_minima3.m
new file mode 100644
index 0000000000000000000000000000000000000000..f90b25908e6173d0470a6dd26f2c3243a6e05bf1
--- /dev/null
+++ b/calibration/local_minima3.m
@@ -0,0 +1,22 @@
+function locmin = local_minima3(X)
+ % return bool array of points that are local minima in the array x
+ sz = size(X);
+ rx = 2 : sz(1) - 1;
+ ry = 2 : sz(2) - 1;
+ rz = 2 : sz(3) - 1;
+
+ locmin = true(sz-2);
+
+ for i = -1 : 1
+ for j = -1 : 1
+ for k = -1 : 1
+ if i == j && j == k
+ continue;
+ end
+
+ locmin = locmin & X(rx,ry,rz) < X(rx+i,ry+j,rz+k);
+ end
+ end
+ end
+ locmin = padarray(locmin, [1 1 1], false, 'both');
+end
diff --git a/calibration/log_find_markers.m b/calibration/log_find_markers.m
new file mode 100644
index 0000000000000000000000000000000000000000..4ad8c50319867f24ebb45b286d648d96f469c733
--- /dev/null
+++ b/calibration/log_find_markers.m
@@ -0,0 +1,69 @@
+function [ij_mark, im_filt] = log_find_markers(i_axis, j_axis, im, scale, n_max, filter_type)
+ % use Laplacian of Gaussian filter to find markers in image im
+ % at lengthscale scale.
+ %
+ % returns n_max locations where the LoG filter is a local maximum
+ % ordered by magnitude of the peak
+ %
+ % local quadratic peak finding is applied to get accurate locations of
+ % maxima
+
+ %% define filter
+ if nargin < 6
+ filter_type = 'log';
+ end
+ if strcmp(filter_type, 'log')
+ % default laplacian of Gaussian filter
+ kernel_size = round(scale*[1 1]*6);
+ kernel_size = 2*floor(kernel_size/2) + 1; % always use odd kernel
+ G = fspecial('log', kernel_size, scale);
+ elseif strcmp(filter_type, 'disk')
+ % disk filter
+ G = -fspecial('disk', round(scale));
+ kernel_size = size(G);
+ end
+ im_filt = single(conv2(im, G, 'same'));
+
+ %% identify local maxima
+ % grid
+ Ni = length(i_axis);
+ Nj = length(j_axis);
+ [imat,jmat] = ndgrid(i_axis, j_axis);
+ % search for local maximum in neighbourhood of kernel
+ im_filt_max = maxfiltnd(im_filt, kernel_size); % sliding maximum filter over kernel_size area
+ local_max = im_filt == im_filt_max;
+ peak_mag = im_filt(local_max);
+ % identify locations of local maxima
+ [pli,plj] = find(local_max);
+ ij_mark = [imat(local_max), jmat(local_max)];
+ % sort by size of peak
+ [~,reidx] = sort(peak_mag, 'descend');
+ ij_mark = ij_mark(reidx, :);
+ plij = [pli(reidx), plj(reidx)];
+
+ % exclude those near edges
+ edge = round(scale);
+ i_lim = sort(i_axis([edge+1 end-edge]), 'ascend');
+ j_lim = sort(j_axis([edge+1 end-edge]), 'ascend');
+ near_edge = ij_mark(:,1) < i_lim(1) | ij_mark(:,1) > i_lim(2) ...
+ | ij_mark(:,2) < j_lim(1) | ij_mark(:,2) > j_lim(2);
+ ij_mark = ij_mark(~near_edge,:);
+ plij = plij(~near_edge, :);
+
+ %% recover n_max largest local maxima
+ n_mark = min(n_max, size(ij_mark,1));
+ ij_mark = ij_mark(1:n_mark, :)';
+ plij = plij(1:n_mark,:);
+
+ %% apply local quadratic peak finding
+ rng = (-edge : edge);
+ di = (i_axis(end) - i_axis(1))/(Ni-1);
+ dj = (j_axis(end) - j_axis(1))/(Nj-1);
+ for n = 1 : n_mark
+ p_x = polyfit(rng*di, im_filt(plij(n,1) + rng, plij(n,2))', 2);
+ p_y = polyfit(rng*dj, im_filt(plij(n,1), plij(n,2) + rng) , 2);
+ ij_mark(1,n) = ij_mark(1,n) - p_x(2) / (2*p_x(1));
+ ij_mark(2,n) = ij_mark(2,n) - p_y(2) / (2*p_y(1));
+ end
+end
+
\ No newline at end of file
diff --git a/calibration/log_test.m b/calibration/log_test.m
new file mode 100644
index 0000000000000000000000000000000000000000..cf3bb997ec78e95f46cac8fdd2b3f076bec361f7
--- /dev/null
+++ b/calibration/log_test.m
@@ -0,0 +1,179 @@
+%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
+% test laplacian of Gaussian method for automatic identification of markers
+% in image
+
+
+blob_scale = 3;
+grid_tol = 0.2;
+n_iter = 3;
+grid_size_x = 21; % must be odd #
+grid_size_y = 21; % must be odd #
+img_file = '/data/LowRe-2013/calib/CAMERA-01/CAM1_000001.tif';
+
+%% load image
+im = imread(img_file)';
+im = double(im);
+im = fliplr(im);
+sz = size(im);
+Ni = sz(1);
+Nj = sz(2);
+i_axis = 1 : sz(1);
+j_axis = 1 : sz(2);
+[imat,jmat] = ndgrid(i_axis,j_axis);
+
+%% filter image at twice blob scale, get 3 big markers
+[ij_big,im_filt] = log_find_markers(i_axis, j_axis, im, 2*blob_scale, 3);
+
+% identify top, right and center markers
+% calculate unit vectors corresponding to each edge
+normals = [(ij_big(:,3) - ij_big(:,2))' / norm(ij_big(:,3) - ij_big(:,2));
+ (ij_big(:,3) - ij_big(:,1))' / norm(ij_big(:,3) - ij_big(:,1));
+ (ij_big(:,2) - ij_big(:,1))' / norm(ij_big(:,2) - ij_big(:,1))];
+% if n_12 is nearly parallel to y |n_12.y| ~= 1
+% then vertex 3 is the right hand vertex. use this trick to find right hand
+% vertex
+[~,i_right] = max(abs(normals(:,2)));
+% remaining vertices are top & center. top is higher up in image than
+% center
+i_tb = setdiff(1:3, i_right);
+if ij_big(2, i_tb(1)) > ij_big(2, i_tb(2))
+ i_top = i_tb(1);
+ i_center = i_tb(2);
+else
+ i_top = i_tb(2);
+ i_center = i_tb(1);
+end
+
+ij_center = ij_big(:,i_center);
+ij_top = ij_big(:,i_top);
+ij_right = ij_big(:,i_right);
+ij_big = [ij_center, ij_right, ij_top];
+xy_big = [[0;0], [1;0], [0;1]];
+
+% calculate included area of triangle
+tri_area_px = abs(ij_big(1,1)*(ij_big(2,2) - ij_big(2,3)) ...
+ + ij_big(1,2)*(ij_big(2,3) - ij_big(2,1)) ...
+ + ij_big(1,3)*(ij_big(2,1) - ij_big(2,2))) / 2;
+tri_area_mm = 0.5;
+n_markers_est = floor(Ni * Nj / tri_area_px / 2);
+search_tolerance = 0.05 * norm(ij_top - ij_right);
+scale_px_to_mm = sqrt(tri_area_px / tri_area_mm);
+
+%% debug: plot identified markers
+figure(1);
+clf;
+imagesc(i_axis, j_axis, im_filt');
+hold on;
+plot(ij_center(1), ij_center(2), 'ro');
+plot(ij_top(1), ij_top(2), 'go');
+plot(ij_right(1), ij_right(2), 'bo');
+legend('center', 'top', 'right');
+%plot(ij_markers(1,:), ij_markers(2,:), 'k.');
+
+axis equal tight xy;
+colormap gray;
+xlim(i_axis([1 end]));
+ylim(j_axis([1 end]));
+
+%% filter image at blob scale, find ordinary markers
+[ij_ordinary,im_filt]= log_find_markers(i_axis, j_axis, im, blob_scale, n_markers_est);
+% exclude those near cent025er, top + right markers
+d_center = sqrt(sum(bsxfun(@minus, ij_ordinary, ij_center).^2, 1));
+d_right = sqrt(sum(bsxfun(@minus, ij_ordinary, ij_right ).^2, 1));
+d_top = sqrt(sum(bsxfun(@minus, ij_ordinary, ij_top ).^2, 1));
+too_near = d_center < search_tolerance*4 ...
+ | d_right < search_tolerance*4 ...
+ | d_top < search_tolerance*4;
+ij_ordinary = ij_ordinary(:, ~too_near);
+% combine set with location of top, center and right markers
+ij_markers = [ij_center, ij_right, ij_top, ij_ordinary];
+
+%% determine affine transformation, select extra markers
+A_xy_to_ij = [ij_right-ij_center, ij_top-ij_center];
+xy_markers = A_xy_to_ij \ bsxfun(@minus, ij_markers, ij_center);
+
+% choose a set of nearby markers
+x_near = -1 : 1;
+y_near = x_near;
+[xm_near,ym_near] = ndgrid(x_near, y_near);
+xy_near = [xm_near(:), ym_near(:)]';
+
+% find matching markers and calcuate pinhole model
+[xy_near,ij_near] = search_markers(xy_near, ij_markers, search_tolerance, 'affine', A_xy_to_ij, ij_center);
+n_near = size(xy_near, 2);
+[P2D, ij_res] = p2d_fit(xy_near, ij_near);
+
+%% iteratively refine model, search for more markers
+max_grid_size = max(grid_size_x, grid_size_y);
+for grid_size = 3 : 2 : max_grid_size
+ %% choose a set of nearby markers to search for
+ search_size_x = min(grid_size, grid_size_x);
+ search_size_y = min(grid_size, grid_size_y);
+ x_near = -(search_size_x-1)/2 : +(search_size_x-1)/2;
+ y_near = -(search_size_y-1)/2 : +(search_size_y-1)/2;
+ [xm_near,ym_near] = ndgrid(x_near, y_near);
+ xy_near = [xm_near(:), ym_near(:)]';
+
+ %% find matching markers and refine pinhole model
+ [xy_near,ij_near,ij_pred] = search_markers(xy_near, ij_markers, search_tolerance, 'pinhole', P2D);
+ n_near = size(xy_near, 2);
+ [P2D, ij_res] = p2d_fit(xy_near, ij_near);
+end
+
+%% report statistics of residuals
+fprintf('found %d markers\n', n_near);
+fprintf('RMS residual:\n');
+fprintf(' x %0.3f px\n', sqrt(mean(ij_res(1,:).^2)));
+fprintf(' y %0.3f px\n', sqrt(mean(ij_res(2,:).^2)));
+fprintf('Largest residual:\n');
+fprintf(' x %0.3f px\n', max(abs(ij_res(1,:))));
+fprintf(' y %0.3f px\n', max(abs(ij_res(2,:))));
+
+%% dewarp with pinhole model
+x_lim = [min(xy_near(1,:)), max(xy_near(1,:))];
+y_lim = [min(xy_near(2,:)), max(xy_near(2,:))];
+Nx = round((x_lim(2)-x_lim(1))*scale_px_to_mm);
+Ny = round((y_lim(2)-y_lim(1))*scale_px_to_mm);
+x_axis = linspace(x_lim(1), x_lim(2), Nx);
+y_axis = linspace(y_lim(1), y_lim(2), Ny);
+[imat_dewarped,jmat_dewarped] = p2d_dewarp(P2D, x_axis, y_axis);
+im_dewarped = interp2(i_axis, j_axis, im', imat_dewarped, jmat_dewarped);
+
+xy_near_true = p2d_map_im2obj(P2D, ij_near);
+xy_big = p2d_map_im2obj(P2D, ij_big);
+xy_center = xy_big(:,1);
+xy_right = xy_big(:,2);
+xy_top = xy_big(:,3);
+
+%% plot
+figure(2);
+clf;
+imagesc(x_axis, y_axis, im_dewarped');
+% plot markers
+hold on;
+plot(xy_near_true(1,:), xy_near_true(2,:), 'r.');
+viscircles(xy_center', 0.1, 'DrawBackgroundCircle', true, 'EdgeColor', 'red');
+viscircles(xy_right' , 0.1, 'DrawBackgroundCircle', true, 'EdgeColor', 'blue');
+viscircles(xy_top' , 0.1, 'DrawBackgroundCircle', true, 'EdgeColor', 'green');
+
+axis equal tight xy;
+xlim(x_lim);
+ylim(y_lim);
+colormap gray;
+grid on;
+set(gca, 'xtick', x_lim(1) : x_lim(2));
+set(gca, 'ytick', y_lim(1) : y_lim(2));
+%{
+%plot(ij_center(1), ij_center(2), 'rx');
+%plot(ij_top(1), ij_top(2), 'gx');
+%plot(ij_right(1), ij_right(2), 'kx');
+%plot(ij_markers(1,:), ij_markers(2,:), 'k.');
+plot(ij_near(1,:), ij_near(2,:), 'rx');
+viscircles(ij_pred', search_tolerance*ones(n_near,1));
+for n = 1 : n_near
+ text(ij_near(1,n)+20, ij_near(2,n), sprintf('%d,%d', xy_near(:,n)));
+end
+axis equal tight xy;
+xlim(i_axis([1 end]));
+ylim(j_axis([1 end]));
+%}
\ No newline at end of file
diff --git a/calibration/make_dark.m b/calibration/make_dark.m
new file mode 100644
index 0000000000000000000000000000000000000000..c72494cdc6928bb2e84d7abb58bca9e28bde6443
--- /dev/null
+++ b/calibration/make_dark.m
@@ -0,0 +1,92 @@
+%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
+% generate a dark image based on particle tracking movies
+
+addpath([pwd '/../gmv']);
+
+%% input parameters
+root_dir = '/scratch21/lawson/iso960/';
+gmv_pat = '%s/gmv/series_%04d.cam%d.gmv';
+dark_pat = '%s/calib/dark_160803.cam%d.gmv';
+
+cam_idx = 0;
+series_range = 0 : 40 : 2999;
+frame_skip = 35;
+c_lim = [0 128];
+
+%% load first image
+gmv_name = sprintf(gmv_pat,root_dir,series_range(1),cam_idx);
+[dark_img,header] = read_gmv(gmv_name, 0, 1);
+threshold = floor(header.threshold / 256);
+dark_img = ones(size(dark_img), 'uint16') * threshold;
+n_frames = header.n_frames;
+
+%% iterate over series
+n_series = length(series_range);
+for is = 1 : n_series
+ %% report
+ series_idx = series_range(is);
+ gmv_name = sprintf(gmv_pat,root_dir,series_idx,cam_idx);
+ fprintf('Series %4d: %s\n', series_idx, gmv_name);
+
+ %% load frames
+ for f = 0 : frame_skip : n_frames-1
+ frm = read_gmv(gmv_name, f, 1);
+ frm = uint16(frm);
+ nz = frm ~= 0;
+ dark_img(nz) = min(dark_img(nz), frm(nz));
+ end
+end
+
+%% save dark image
+dark_name = sprintf(dark_pat, root_dir, cam_idx);
+fprintf('Saving to %s\n', dark_name);
+b_write = false;
+if exist(dark_name, 'file')
+ str = input('File already exists. Overwrite? y/n ', 's');
+ if strcmp(str, 'y')
+ b_write = true;
+ end
+else
+ b_write = true;
+end
+
+if b_write
+ save_gmv(dark_name, dark_img, header);
+end
+fprintf('done\n');
+
+%% plot
+
+figure(1);
+imagesc(dark_img');
+ax = gca;
+axis equal tight ij;
+set(gca, 'clim', [0 threshold]);
+colormap(gray);
+colorbar;
+title(sprintf('Dark image of camera %d', cam_idx));
+
+%% plot last frame with dark correction
+frm_corr = frm - dark_img;
+figure(2);
+imagesc(frm_corr');
+ax(2)=gca;
+axis equal tight ij;
+set(gca, 'clim', c_lim);
+colormap(1-gray);
+colorbar;
+title(sprintf('Corrected image from camera %d', cam_idx));
+
+%% plot last frame without dark correction
+figure(3);
+imagesc(frm');
+ax(3)=gca;
+axis equal tight ij;
+set(gca, 'clim', c_lim);
+colormap(1-gray);
+colorbar;
+title(sprintf('Uncorrected image from camera %d', cam_idx));
+
+%% link axes
+linkaxes(ax, 'xy');
+
diff --git a/calibration/manual_sheet_thickness.m b/calibration/manual_sheet_thickness.m
new file mode 100644
index 0000000000000000000000000000000000000000..d027111ad07bc1e9d19cf329695a8943a379430d
--- /dev/null
+++ b/calibration/manual_sheet_thickness.m
@@ -0,0 +1,61 @@
+%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
+% manually measure laser sheet thickness
+
+im_file = '/nfs/scratch23/lawson/K62/170119/sheet/sheet_thickness_9213.tif';
+
+%% load image
+im = imread(im_file);
+im = im(:,:,1);
+im = double(im);
+
+%% plot
+figure(1);
+clf;
+ax = [0,0];
+ax(2) = subplot(1,2,2);
+ax(1) = subplot(1,2,1);
+
+imagesc(im);
+axis equal tight ij;
+
+%% get points
+fprintf('Select 2 points\n');
+ij_points = ginput(2);
+ij_start = round(ij_points(1,:));
+ij_end = round(ij_points(2,:));
+ij_end = [ij_end(1), ij_start(2)];
+ij_points = [ij_start; ij_end];
+
+%% get brightness and fit
+i_axis = ij_start(1) : ij_end(1);
+j_axis = ij_start(2) * ones(size(i_axis));
+line_profile= im(ij_start(2), i_axis);
+
+ft = fittype('A*exp( -((x-x0)/s).^2 ) + C', 'indep', 'x', 'coefficients', {'A', 'x0', 'C', 's'});
+
+% initial guess
+A0 = max(line_profile) - min(line_profile);
+C0 = min(line_profile);
+x0 = mean(i_axis);
+s0 = max(i_axis) - min(i_axis);
+
+P0 = [A0, x0, C0, s0];
+fitfun = fit(i_axis(:), line_profile(:), ft, 'StartPoint', P0);
+
+sigma = fitfun.s;
+width = 2*sqrt(2)*sigma;
+
+%% report
+fprintf('Measured at y = %d px\n', ij_start(2));
+fprintf('Thickness: %0.1f px\n', sigma);
+fprintf('e^-2 thickness: %0.1f px\n', width);
+
+%% plot points
+hold(ax(1), 'on');
+
+plot(ax(1), ij_points(:,1), ij_points(:,2), 'rx-');
+
+hold(ax(2), 'off');
+plot(ax(2), i_axis, line_profile, 'kx');
+hold(ax(2), 'on');
+plot(ax(2), i_axis, fitfun(i_axis), 'k-');
\ No newline at end of file
diff --git a/calibration/min_bounding_box.m b/calibration/min_bounding_box.m
new file mode 100644
index 0000000000000000000000000000000000000000..d5485a7dc3886b5579549bae62eb076bfd7df063
--- /dev/null
+++ b/calibration/min_bounding_box.m
@@ -0,0 +1,79 @@
+function [bb_lim, R, A, b] = min_bounding_box(camera)
+ % [bb_lim, R] = min_bounding_box(calib)
+ % determine the corners of the smallest arbitrarily oriented bounding box
+ % which fit encapsulate the extent of the measurement volume
+ %
+ % bb_lim is a 2 x 3 matrix containing the x, y and z limits of the
+ % smallest bounding box which encapsulate the entire measurement volume
+ % in a rotated coordinate system
+ %
+ % R is a rotation matrix describing the rotation of object space
+ % coordinates into a coordinate system in which all points fit within
+ % the limits
+
+ %% get view frustums
+ % M = M_ary(:,p,c) are the coefficients for the equation of plane p of the
+ % viewing frustum for camera c.
+ % i.e.
+ % M(1)x + M(2)y + M(3)z + M(4) >= 0
+ % when a point lies within the measurement volume
+ n_cameras = length(camera);
+ M_ary = zeros(4,4,n_cameras);
+ U_ary = zeros(3,4,n_cameras);
+ for c = 1 : n_cameras
+ [M,U] = frustum(camera(c));
+ M_ary(:,:,c)= M';
+ U_ary(:,:,c)= U;
+ end
+
+ %% get orientation of view frustum
+ R_ary = zeros(3,3,n_cameras);
+ normalise = @(x) x / norm(x);
+ for c = 1 : n_cameras
+ e1 = normalise(U_ary(:,1,c));
+ e2 = normalise(U_ary(:,2,c) - U_ary(:,1,c));
+ e3 = normalise(cross(e1,e2));
+ R_ary(:,:,c)= [e1, e2, e3];
+ end
+
+ %% setup linear programming problem
+ % to obtain volume bounds, we find the x that
+ % minimises c'x subject to Ax < b
+ A = reshape(-M_ary(1:3,:,:), [3 4*n_cameras])';
+ b = reshape( M_ary(4 ,:,:), [4*n_cameras 1]);
+ bb_lim = zeros(2,3,n_cameras);
+ bb_vol = zeros(1,n_cameras);
+ opts = optimoptions('linprog', 'Display', 'off');
+
+ %% try orientations corresponding to orientation of cameras
+ for c = 1 : n_cameras
+ for i = 1 : 3
+ e_i = R_ary(:,i,c);
+ bb_lim(1,i,c) = dot(e_i, linprog(+e_i, A, b, [], [], [], [], [], opts));
+ bb_lim(2,i,c) = dot(e_i, linprog(-e_i, A, b, [], [], [], [], [], opts));
+ end
+ bb_size = bb_lim(2,:,c) - bb_lim(1,:,c);
+ bb_vol(c) = abs(prod(bb_size));
+ end
+
+ %% identify smallest volume bounding box
+ [~,c_best] = min(bb_vol);
+ R = R_ary(:,:,c_best);
+ bb_lim = bb_lim(:,:,c_best);
+
+ %% test
+ %{
+ N = 1E5;
+ bb_size = bb_lim(2,:) - bb_lim(1,:);
+ bb_pos = bsxfun(@plus, bb_lim(1,:)', diag(bb_size) * rand(3,N));
+ gl_pos = R*bb_pos;
+ bb_dist = bsxfun(@minus, A*gl_pos, b);
+ b_inside = all(bb_dist <= 0, 1);
+ figure(1);
+ clf;
+ plot3(gl_pos(1, b_inside), gl_pos(2, b_inside), gl_pos(3, b_inside), 'g.');
+ hold on;
+ plot3(gl_pos(1,~b_inside), gl_pos(2,~b_inside), gl_pos(3,~b_inside), 'r.');
+ axis equal tight xy;
+ %}
+end
\ No newline at end of file
diff --git a/calibration/p2d_dewarp.m b/calibration/p2d_dewarp.m
new file mode 100644
index 0000000000000000000000000000000000000000..7cac550967a1552799f0d7f404439d45a4f36884
--- /dev/null
+++ b/calibration/p2d_dewarp.m
@@ -0,0 +1,9 @@
+function [imat,jmat] = p2d_dewarp(P2D, x_axis, y_axis)
+ Nx = length(x_axis);
+ Ny = length(y_axis);
+ [xmat, ymat] = ndgrid(x_axis, y_axis);
+ xy = [xmat(:), ymat(:)]';
+ ij = p2d_map_obj2im(P2D, xy);
+ imat = reshape(ij(1,:), [Nx Ny]);
+ jmat = reshape(ij(2,:), [Nx Ny]);
+end
\ No newline at end of file
diff --git a/calibration/plot_calibration_residuals.m b/calibration/plot_calibration_residuals.m
new file mode 100644
index 0000000000000000000000000000000000000000..2378ee9069a321a6badfbe52b1fa85189e7f60c9
--- /dev/null
+++ b/calibration/plot_calibration_residuals.m
@@ -0,0 +1,57 @@
+%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
+% plot residuals in calibration procedure, comparing projected points
+% according to model and observed points
+
+addpath([pwd '/../calibration-mpids/']);
+
+%% input parameters
+calib_dir = '/nfs/scratch21/lawson/161019/calib/';
+calib_file_mat = [calib_dir '/camera-model-original.mat'];
+font_size = 20;
+pixel_pitch_mm = [10E-3 10E-3];
+
+%% load calibration
+fs = importdata(calib_file_mat);
+n_cameras = fs.n_cameras;
+camera = fs.camera;
+
+%% iterate over cameras
+for c = 1 : n_cameras
+ %% load parameters
+ i_axis = camera(c).i_axis;
+ j_axis = camera(c).j_axis;
+ Ni = length(i_axis);
+ Nj = length(j_axis);
+
+ %% load markers and calculate residuals
+ gl_markers = camera(c).calib.gl_markers;
+ ij_markers = camera(c).calib.ij_markers;
+ ij_markers_proj = pinhole_model(gl_markers, camera(c).calib, true);
+ ij_markers_res = ij_markers - ij_markers_proj;
+
+ %% Tsai calibration
+ cparams = struct('Npixh', Nj, 'Npixw', Ni, ...
+ 'hpix', pixel_pitch_mm(1), ...
+ 'wpix', pixel_pitch_mm(2));
+ [tsai_calib, tsai_res] = calib_Tsai(ij_markers', gl_markers', cparams);
+ tsai_res = -tsai_res';
+
+ %% plot
+ title_str = sprintf('Camera %d', c);
+
+ figure(c);
+ clf;
+ quiver(ij_markers(1,:), ij_markers(2,:), ...
+ ij_markers_res(1,:), ij_markers_res(2,:), 'b');
+ %hold on;
+ %quiver(ij_markers(1,:), ij_markers(2,:), ...
+ % tsai_res(1,:), tsai_res(2,:), 'r');
+ axis equal tight xy;
+ xlim(i_axis([1 end]));
+ ylim(j_axis([1 end]));
+
+ % make pretty
+ title(title_str, 'interpreter', 'latex', 'fontsize', font_size);
+ xlabel('$i$ / px', 'interpreter', 'latex', 'fontsize', font_size);
+ ylabel('$j$ / px', 'interpreter', 'latex', 'fontsize', font_size);
+end
\ No newline at end of file
diff --git a/calibration/plot_camera_calibration_3d.m b/calibration/plot_camera_calibration_3d.m
new file mode 100644
index 0000000000000000000000000000000000000000..1c8e9996429067235e3fd79a6b5e022a45a007be
--- /dev/null
+++ b/calibration/plot_camera_calibration_3d.m
@@ -0,0 +1,202 @@
+%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
+% camera_calibration.m
+%
+% create a pinhole camera calibration for one or more cameras
+% from a set of views of the same calibration plate in different positions
+% a compensation is made for error in calibration plate position and
+% orientation
+
+addpath([pwd '/pinhole/']);
+%% input parameters
+camera_model_file = '/scratch21/lawson/p1344_kMeta2/calib/camera-model-160728-0000-otf-gauss2.mat';
+font_size = 20;
+
+%% load model
+setup = importdata(camera_model_file);
+
+% camera calibration
+n_cameras = setup.n_cameras;
+n_views = setup.n_views;
+
+% plate pose
+gl_plate_pos = setup.gl_plate_pos;
+pl_to_gl_mat = repmat(setup.pl_to_gl, [1 1 n_views]);
+camera_model = setup.camera;
+gl_to_re = eye(3);
+gl_origin = setup.gl_origin;
+r_sphere = min(setup.r_sphere);
+
+%% get bounding frustums
+M_ary = zeros(4,4,n_cameras); % equation of plane
+U_ary = zeros(3,4,n_cameras); % unit vectors of viewing pyramid edges
+xc_ary = zeros(3,n_cameras); % camera origin
+xf_ary = zeros(3,4,n_cameras); % points on corners of viewing pyramid
+for c = 1 : n_cameras
+ [M,U,xc] = frustum(setup.camera(c));
+ M_ary(:,:,c) = M;
+ U_ary(:,:,c) = U;
+ xc_ary(:,c) = xc;
+ % get four corners of view frustum
+ for p = 1 : 4
+ len = dot(gl_origin - xc, U(:,p));
+ xf_ary(:,p,c) = xc + (len + 2*r_sphere)*U(:,p);
+ end
+end
+
+%% determine bounding boxes
+[gl_bb_lim, gl_bb_A, gl_bb_b]= bounding_box(setup.camera);
+[xg,yg,zg] = ndgrid(linspace(gl_bb_lim(1,1), gl_bb_lim(2,1), 100), ...
+ linspace(gl_bb_lim(1,2), gl_bb_lim(2,2), 100), ...
+ linspace(gl_bb_lim(1,3), gl_bb_lim(2,3), 100));
+gl_pts = [xg(:), yg(:), zg(:)]';
+bb_dist = bsxfun(@minus, gl_bb_A*gl_pts, gl_bb_b);
+b_inside = all(bb_dist <= 0, 1);
+gl_inside = gl_pts(:,b_inside);
+
+% determine minimum bounding box
+[mv_bb_lim, mv_to_gl, mv_bb_A, mv_bb_b] = min_bounding_box(setup.camera);
+mv_bb_size = mv_bb_lim(2,:) - mv_bb_lim(1,:);
+
+% save in setup
+setup.gl_bb_lim = gl_bb_lim;
+setup.gl_bb_A = gl_bb_A;
+setup.gl_bb_b = gl_bb_b;
+setup.mv_bb_lim = mv_bb_lim;
+setup.mv_to_gl = mv_to_gl;
+setup.mv_bb_A = mv_bb_A;
+setup.mv_bb_b = mv_bb_b;
+
+%% convex bounding hull
+[tri_hull, V_hull] = convhull(gl_inside(1,:)', gl_inside(2,:)', gl_inside(3,:)');
+fprintf('Convex hull has volume %0.1f mm^2\n', V_hull);
+
+%% calculate coordinates of inscribed sphere
+theta = linspace(0, pi, 21);
+phi = linspace(0, 2*pi, 41);
+[tmat,pmat] = ndgrid(theta,phi);
+en_sphere = [sin(tmat(:)').*cos(pmat(:)');
+ sin(tmat(:)').*sin(pmat(:)');
+ cos(tmat(:)')];
+gl_sphere = bsxfun(@plus, r_sphere * en_sphere, gl_origin);
+
+%% project inscribed sphere onto different views
+n_sphere = size(gl_sphere, 2);
+ij_sphere_ary = zeros(2, n_sphere, n_cameras);
+ij_orgin_ary = zeros(2, n_cameras);
+for c = 1 : n_cameras
+ ij_sphere_ary(:,:,c) = pinhole_model(gl_sphere, setup.camera(c).calib, true);
+ ij_origin_ary(:,c) = pinhole_model(gl_origin, setup.camera(c).calib, true);
+end
+
+%% plot 3D view of frustum and inscribed sphere
+figure(1);
+clf;
+hold on;
+
+% inscribed sphere
+plot3(gl_sphere(1,:), gl_sphere(2,:), gl_sphere(3,:), 'k.');
+% convex bounding hull
+S = trisurf(tri_hull, gl_inside(1,:), gl_inside(2,:), gl_inside(3,:));
+set(S, 'FaceColor', 'r', 'facealpha', 0.3, 'edgecolor', 'none');
+
+% viewing frustum of each camera
+%{
+colours = {'red', 'green', 'blue'};
+for c = 1 : n_cameras
+ plot3(xc_ary(1,c), xc_ary(2,c), xc_ary(3,c), 'o', 'Color', colours{c});
+ for p = 1 : 4
+ tri = [xc_ary(:,c), xf_ary(:,p,c), xf_ary(:, mod(p,4)+1, c)];
+ P = patch(tri(1,:), tri(2,:), tri(3,:), colours{c});
+ set(P, 'facealpha', 0.2);
+ end
+end
+%}
+
+set(gca, 'CameraTarget', gl_origin);
+% plot settings
+axis equal tight xy
+xlim(gl_origin(1) + 3*r_sphere*[-1 1]);
+ylim(gl_origin(2) + 3*r_sphere*[-1 1]);
+zlim(gl_origin(3) + 3*r_sphere*[-1 1]);
+xlabel('x [mm]', 'interpreter', 'latex', 'fontsize', font_size);
+ylabel('y [mm]', 'interpreter', 'latex', 'fontsize', font_size);
+zlabel('z [mm]', 'interpreter', 'latex', 'fontsize', font_size);
+
+%% plot the projection in each camera
+figure(2);
+clf;
+
+for c = 1 : n_cameras
+ %% bits
+ i_axis = setup.camera(c).i_axis;
+ j_axis = setup.camera(c).j_axis;
+ ij_sphere = ij_sphere_ary(:,:,c);
+ ij_origin = ij_origin_ary(:,c);
+ id = setup.camera(c).id;
+ titstr = sprintf('Camera %d, id = %d', c, id);
+
+ %% plot
+ subplot(2,2,c);
+ cla;
+ hold on;
+ plot(ij_sphere(1,:), ij_sphere(2,:), 'k.');
+ plot(ij_origin(1), ij_origin(2), 'ro');
+ axis equal tight xy;
+ xlim(sort(i_axis([1 end])));
+ ylim(sort(j_axis([1 end])));
+ title(titstr, 'interpreter', 'latex', 'fontsize', font_size);
+ xlabel('i / px', 'interpreter', 'latex', 'fontsize', font_size);
+ xlabel('j / px', 'interpreter', 'latex', 'fontsize', font_size);
+end
+
+%% calculate marker positions for each view
+re_marker_ary = cell(1,2);
+
+for c = 1 : 2
+ for v = 1 : n_views
+ uv_markers = camera_model(c).view(v).uv_markers;
+ n_markers = size(uv_markers, 2);
+ %% append to vectors
+ gl_pos = gl_plate_pos(:,v);
+ pl_to_gl = pl_to_gl_mat(:,:,v);
+
+ u_marker = uv_markers(1,:);
+ v_marker = uv_markers(2,:);
+ gl_markers = bsxfun(@plus, gl_pos, pl_to_gl * [u_marker; v_marker; zeros(1,n_markers)]);
+ re_markers = gl_to_re * bsxfun(@plus, -gl_origin, gl_markers);
+
+ re_marker_ary{c} = [re_marker_ary{c}, re_markers];
+ end
+end
+
+% find markers in camera 1 only, camera 2 only and both
+re_marker_c1 = setdiff(re_marker_ary{1}', re_marker_ary{2}', 'rows')';
+re_marker_c2 = setdiff(re_marker_ary{2}', re_marker_ary{1}', 'rows')';
+re_marker_c12 = intersect(re_marker_ary{1}', re_marker_ary{2}', 'rows')';
+
+%% plot
+figure(10);
+clf;
+hold on;
+
+% plot markers
+plot3(re_marker_c1(1,:), re_marker_c1(2,:), re_marker_c1(3,:), '.', 'color', [1 0 0]);
+plot3(re_marker_c2(1,:), re_marker_c2(2,:), re_marker_c2(3,:), '.', 'color', [0 0 1]);
+plot3(re_marker_c12(1,:), re_marker_c12(2,:), re_marker_c12(3,:), '.', 'color', [0.8 0 0.8]);
+
+% plot corners of reconstruction
+re_corners = [re_x_lim([1 1 1 1 2 2 2 2]);
+ re_y_lim([1 1 2 2 1 1 2 2]);
+ re_z_lim([1 2 1 2 1 2 1 2])];
+plot3(re_corners(1,[1 2 3 4 1]), re_corners(2, [1 2 3 4 1]), re_corners(3, [1 2 3 4 1]), 'k.-', ...
+ re_corners(1,[5 6 7 8 5]), re_corners(2, [5 6 7 8 5]), re_corners(3, [5 6 7 8 5]), 'k.-', ...
+ re_corners(1,[1 3 7 5 1]), re_corners(2, [1 3 7 5 1]), re_corners(3, [1 3 7 5 1]), 'k.-', ...
+ re_corners(1,[2 4 8 6 2]), re_corners(2, [2 4 8 6 2]), re_corners(3, [2 4 8 6 2]), 'k.-');
+
+axis equal tight xy;
+view(3);
+camup([0 1 0]);
+
+%xlabel('$x$ / mm', 'interpreter', 'latex', 'fontsize', 20);
+%ylabel('$y$ / mm', 'interpreter', 'latex', 'fontsize', 20);
+%zlabel('$z$ / mm', 'interpreter', 'latex', 'fontsize', 20);
\ No newline at end of file
diff --git a/calibration/plot_dewarped.m b/calibration/plot_dewarped.m
new file mode 100644
index 0000000000000000000000000000000000000000..07e67165ef1620926395e3b33ee8a63275b9f121
--- /dev/null
+++ b/calibration/plot_dewarped.m
@@ -0,0 +1,102 @@
+%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
+% load a raw image from file and dewarp it with a 2D dewarping specified
+% in the provided calibration file
+
+%% input parameters
+
+img_file = '/data/cluster/160309/9219-003.TIF';
+calib_file = '/data/cluster/160309/CAM1-01.mat';
+x0 = [7, -2];
+
+%% load calibration
+fs = importdata(calib_file);
+P2D = fs.P;
+i_axis = fs.i_axis;
+j_axis = fs.j_axis;
+x_axis = fs.x_axis;
+y_axis = fs.y_axis;
+Ni = length(i_axis);
+Nj = length(j_axis);
+grid_dx = fs.grid_dx;
+grid_dy = fs.grid_dy;
+
+%% load image and dewarp
+[xmat,ymat] = ndgrid(x_axis, y_axis);
+[imat_xy,jmat_xy] = p2d_dewarp(P2D, x_axis, y_axis);
+im_ij = double(fliplr(imread(img_file)'));
+im_xy = interp2(i_axis, j_axis, im_ij', imat_xy, jmat_xy, 'linear', 0);
+
+G = fspecial('gaussian', [33 33], 3);
+im_filt = filter2(G, im_xy);
+
+%% cut a small subregion out
+x_prime = x_axis - x0(1);
+y_prime = y_axis - x0(2);
+xrange = abs(x_prime) < 30;
+yrange = abs(y_prime) < 30;
+im_cut = im_filt(xrange,yrange);
+x_cut = x_prime(xrange);
+y_cut = y_prime(yrange);
+[xmc, ymc] = ndgrid(x_cut, y_cut);
+
+%% plot
+figure(1);
+S = surf(x_cut, y_cut, im_cut'); set(S, 'edgecolor', 'none');
+axis equal tight xy;
+grid on;
+colormap jet;
+
+%% fit gaussian
+fitfun = fittype('A*exp(-((x-x0).^2 + (y-y0).^2)/s^2)', ...
+ 'dependent', 'z', ...
+ 'indep', {'x','y'}, ...
+ 'coeff', {'A', 'x0', 'y0', 's'});
+
+[fm,good] = fit([xmc(:),ymc(:)], im_cut(:), fitfun, 'StartPoint', startpoint);
+im_model = fm(xmc, ymc);
+im_res = im_cut - im_model;
+
+disp(fm);
+
+%% plot solution
+figure(2);
+clf;
+
+subplot(1,2,1);
+cla;
+hold off;
+imagesc(x_cut, y_cut, im_cut');
+hold on;
+[C,h] = contour(x_cut, y_cut, im_model'/fm.A, linspace(0, 1, 11));
+clabel(C,h);
+
+axis equal tight xy;
+grid on;
+colormap jet;
+colorbar;
+
+subplot(1,2,2);
+imagesc(x_cut, y_cut, im_res');
+axis equal tight xy;
+grid on;
+colormap jet;
+colorbar;
+
+%% get points and draw
+%{
+xy_circ = ginput();
+%%
+xy_orig = mean(xy_circ, 1);
+xy_dist = sqrt(sum(bsxfun(@minus,xy_circ, xy_orig).^2, 2));
+radius = mean(xy_dist);
+
+theta = linspace(0,2*pi);
+cost = cos(theta);
+sint = sin(theta);
+
+hold on;
+plot(xy_circ(:,1), xy_circ(:,2), 'rx');
+plot(xy_orig(1), xy_orig(2), 'ro');
+plot(xy_orig(1)+cost*radius, xy_orig(2)+sint*radius, 'r-');
+fprintf('radius = %0.3f mm\n', radius);
+%}
\ No newline at end of file
diff --git a/calibration/plot_dewarped_3d.m b/calibration/plot_dewarped_3d.m
new file mode 100644
index 0000000000000000000000000000000000000000..df88480934d02d3d2b39c78c002a7f7b909178f1
--- /dev/null
+++ b/calibration/plot_dewarped_3d.m
@@ -0,0 +1,51 @@
+%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
+% load a raw image from file and dewarp it with a 2D dewarping specified
+% in the provided calibration file
+
+%% input parameters
+model_file = '/scratch23/lawson/K62/170118/calib/camera-model-original.mat';
+calib_file = '/scratch23/lawson/K62/170118/calib/CAM2-05.mat';
+
+%% load calibration
+model = importdata(model_file);
+fs = importdata(calib_file);
+cam_idx = fs.camera_index;
+cam = model.camera(cam_idx);
+
+i_axis = fs.i_axis;
+j_axis = fs.j_axis;
+Ni = length(i_axis);
+Nj = length(j_axis);
+
+x_axis = fs.x_axis*2;
+y_axis = fs.y_axis*2;
+gl_plate = fs.plate_pos;
+Nx = length(x_axis);
+Ny = length(y_axis);
+im_ij = fs.source_image;
+grid_dx = fs.grid_dx;
+grid_dy = fs.grid_dy;
+
+%% load image and dewarp
+[xmat,ymat,zmat] = ndgrid(x_axis, y_axis, gl_plate(3));
+gl_mat = [xmat(:) ymat(:) zmat(:)]';
+ij_mat = pinhole_model(gl_mat, cam.calib, true);
+imat_xy = reshape(ij_mat(1,:), [Nx Ny]);
+jmat_xy = reshape(ij_mat(2,:), [Nx Ny]);
+
+im_xy = interp2(i_axis, j_axis, im_ij', imat_xy, jmat_xy, 'linear', 0);
+
+G = fspecial('gaussian', [33 33], 3);
+im_filt = filter2(G, im_xy);
+
+%% plot
+figure(1);
+clf;
+
+xtick = min(x_axis) : grid_dx : max(x_axis);
+ytick = min(y_axis) : grid_dy : max(y_axis);
+
+imagesc(x_axis, y_axis, im_xy');
+axis equal tight xy;
+set(gca, 'xtick', xtick, 'ytick', ytick);
+grid on;
\ No newline at end of file
diff --git a/calibration/plot_marker_search.m b/calibration/plot_marker_search.m
new file mode 100644
index 0000000000000000000000000000000000000000..3511ceb44653be0e7ba5bb24f764c9945cda4ced
--- /dev/null
+++ b/calibration/plot_marker_search.m
@@ -0,0 +1,40 @@
+%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
+% make a simple plot of the markers identified by search
+% for a given calibration
+addpath([pwd '/../misc']);
+calib_file = 'D:\bigtank-jul-2013\CAL-20-07-13\CAMERA\CAM1-11.mat';
+
+%% load
+fs = importdata(calib_file);
+i_axis = fs.i_axis;
+j_axis = fs.j_axis;
+ij_markers = fs.ij_markers;
+ij_center = fs.ij_center;
+ij_right = fs.ij_right;
+ij_top = fs.ij_top;
+uv_marker = fs.xy_markers;
+im_source = fs.source_image;
+
+%% plot
+draw_arrow = @(x,y,varargin) quiver( x(1),y(1),x(2)-x(1),y(2)-y(1),0, varargin{:} );
+
+
+figure(1);
+clf;
+imagesc(i_axis, j_axis, im_source');
+axis equal tight xy;
+colormap gray;
+
+hold on;
+plot(ij_markers(1,:), ij_markers(2,:), 'rx', 'markersize', 4);
+plot(ij_center(1), ij_center(2), 'ro', 'MarkerSize', 6);
+plot(ij_right(1), ij_right(2), 'bo', 'MarkerSize', 6);
+plot(ij_top(1), ij_top(2), 'go', 'MarkerSize', 6);
+
+%clim([0 1024]);
+set(gca, 'xtick', []);
+set(gca, 'ytick', []);
+%annotation('arrow', [ij_center(1) ij_right(1)], [ij_center(2) ij_right(2)], '-', 'Color', [0 0 1]);
+%annotation('arrow', [ij_center(1) ij_top(1)], [ij_center(2) ij_top(2)], '-', 'Color', [0 1 0]);
+
+
diff --git a/calibration/plot_residuals.m b/calibration/plot_residuals.m
new file mode 100644
index 0000000000000000000000000000000000000000..c2410d2e8485192012485ff4744dc672e5d1f19c
--- /dev/null
+++ b/calibration/plot_residuals.m
@@ -0,0 +1,16 @@
+file_name_pat = '/data/050216/calib/CAM1-%02d.mat';
+
+for n = 1 : 11
+ file_name = sprintf(file_name_pat, n);
+ load(file_name);
+
+ ij_pred = p2d_map_obj2im(P, xy_markers);
+ res = ij_pred-ij_markers;
+
+ figure(1);
+ clf;
+ quiver(ij_markers(1,:),ij_markers(2,:), res(1,:),res(2,:), 'b-');
+ axis equal tight xy;
+
+ pause();
+end
\ No newline at end of file
diff --git a/calibration/plot_residuals_3d.m b/calibration/plot_residuals_3d.m
new file mode 100644
index 0000000000000000000000000000000000000000..4610e2fb4c8addd3598717ae8282a8d6a077b806
--- /dev/null
+++ b/calibration/plot_residuals_3d.m
@@ -0,0 +1,176 @@
+%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
+% plot residuals of calibration in 3D
+
+addpath([pwd '/pinhole/']);
+
+%% input parameters
+calib_file_pat = '/scratch23/lawson/K62/170118/calib/CAM%d-%02d.mat';
+camera_model_file = '/scratch23/lawson/K62/170118/calib/camera-model-original.mat';
+font_size = 20;
+DO_DEBUG_PLOT = false;
+
+%% load model
+setup = importdata(camera_model_file);
+
+% camera calibration
+n_cameras = setup.n_cameras;
+n_views = setup.n_views;
+
+% plate pose
+gl_plate_pos = setup.gl_plate_pos;
+pl_to_gl_mat = repmat(setup.pl_to_gl, [1 1 n_views]);
+camera_model = setup.camera;
+gl_to_re = eye(3);
+gl_origin = setup.gl_origin;
+r_sphere = min(setup.r_sphere);
+n_views = setup.n_views;
+
+%% calculate coordinates of inscribed sphere
+theta = linspace(0, pi, 21);
+phi = linspace(0, 2*pi, 41);
+[tmat,pmat] = ndgrid(theta,phi);
+en_sphere = [sin(tmat(:)').*cos(pmat(:)');
+ sin(tmat(:)').*sin(pmat(:)');
+ cos(tmat(:)')];
+gl_sphere = bsxfun(@plus, r_sphere * en_sphere, gl_origin);
+
+%% triangulate markers from plate
+gl_markers_ref = [];
+gl_markers_tri = [];
+ij_res_ary = cell(1, n_cameras);
+
+for v = 1 : n_views
+ %% load markers
+ fprintf('View %d\n', v);
+ ij_markers = cell(n_cameras, 1);
+ uv_markers = cell(n_cameras, 1);
+ n_markers = zeros(n_cameras, 1);
+ for c = 1 : n_cameras
+ fname = sprintf(calib_file_pat, c, v);
+ fs = importdata(fname);
+ ij_markers{c} = fs.ij_markers;
+ uv_markers{c} = fs.xy_markers;
+ n_markers(c) = size(fs.ij_markers,2);
+
+ fprintf(' c%d: %d markers\n', c, n_markers(c));
+ end
+
+ %% identify common markers
+ uv_common = uv_markers{1};
+ for c = 2 : n_cameras
+
+ uv_common = intersect(uv_common.', uv_markers{c}.', 'rows').';
+ end
+ n_common = size(uv_common, 2);
+ ij_common = zeros(2, n_common, n_cameras);
+ for c = 1 : n_cameras
+ [~, ia, ib] = intersect(uv_markers{c}.', uv_common.', 'rows');
+ ij_common(:,:,c) = ij_markers{c}(:, ia);
+ end
+ fprintf(' %d common markers\n', n_common);
+
+ %% global coords of common markers
+ pl_to_gl = pl_to_gl_mat(:,:,v);
+ gl_plate = gl_plate_pos(:,v);
+ gl_common = mtimesx(pl_to_gl, [uv_common; zeros(1,n_common)]);
+ gl_common = bsxfun(@plus, gl_plate, gl_common);
+
+ %% triangulate
+ [gl_tri, gl_res, ij_res] = triangulate(setup.camera, ij_common, true);
+
+ %% add to array
+ gl_markers_tri = [gl_markers_tri, gl_tri];
+ gl_markers_ref = [gl_markers_ref, gl_common];
+ for c = 1 : n_cameras
+ ij_res_ary{c} = [ij_res_ary{c}; ij_res(:,c)];
+ end
+
+ %% plot common markers
+ style = {'r.', 'g.', 'b.'};
+ if DO_DEBUG_PLOT
+ figure(1);
+ clf;
+ plot(uv_common(1,:), uv_common(2,:), 'ko');
+ hold on;
+ for c = 1 : n_cameras
+ plot(uv_markers{c}(1,:), uv_markers{c}(2,:), style{c});
+ end
+ axis equal tight xy;
+ make_plot_pretty('$u$ [mm]', '$v$ [mm]', '', 20);
+ end
+end
+ij_res_ary = cell2mat(ij_res_ary);
+
+%% calculate disparity
+gl_disp = gl_markers_tri - gl_markers_ref;
+ij_rms = rms(ij_res_ary, 1);
+ij_max = max(ij_res_ary, [], 1);
+gl_rms = rms(gl_disp, 2);
+gl_max = max(abs(gl_disp), [], 2);
+gl_mean = mean(gl_disp, 2);
+
+%% report
+fprintf('Disparity after triangulation:\n');
+fprintf(' c%d: %0.3f px rms %0.3f px max\n', [1:n_cameras; ij_rms; ij_max]);
+fprintf(' x: %6.3f um rms %6.3f um max %+6.3f um mean\n', 1000*gl_rms(1), 1000*gl_max(1), 1000*gl_mean(1));
+fprintf(' y: %6.3f um rms %6.3f um max %+6.3f um mean\n', 1000*gl_rms(2), 1000*gl_max(2), 1000*gl_mean(2));
+fprintf(' z: %6.3f um rms %6.3f um max %+6.3f um mean\n', 1000*gl_rms(3), 1000*gl_max(3), 1000*gl_mean(3));
+
+%% plot markers and their triangulation
+quiver_scale = 40;
+figure(2);
+clf;
+quiver3sc( gl_markers_ref(1,:), gl_markers_ref(2,:), gl_markers_ref(3,:), ...
+ gl_disp(1,:), gl_disp(2,:), gl_disp(3,:), quiver_scale, 'k-');
+hold on;
+plot3(gl_sphere(1,:), gl_sphere(2,:), gl_sphere(3,:), 'b.');
+
+axis equal tight;
+xlabel('$x$ [mm]', 'interpreter', 'latex', 'fontsize', font_size);
+ylabel('$y$ [mm]', 'interpreter', 'latex', 'fontsize', font_size);
+zlabel('$z$ [mm]', 'interpreter', 'latex', 'fontsize', font_size);
+
+%% triangulate markers using calibration
+error('stop');
+
+gl_ref = [];
+gl_meas = [];
+ij_meas = [];
+
+for v = 1 : n_views
+ %% get plate, image and world coordinates of markers
+ pl_to_gl = pl_to_gl_mat(:,:,v);
+ gl_pos = gl_plate_pos(:,v);
+ uv_mark = camera.view(v).uv_markers;
+ n_mark = size(uv_mark, 2);
+ gl_mark = bsxfun(@plus, gl_pos, pl_to_gl*[uv_mark; zeros(1,n_mark)]);
+ ij_mark = camera.view(v).ij_markers;
+
+ %% calculate equation of plane
+ n = pl_to_gl(:,3);
+ d = -dot(gl_pos, n);
+ M = [n; d];
+
+ %% back-project
+ gl_proj = back_project_1c(camera.calib, M, ij_mark, true);
+
+ %% save
+ gl_ref = [gl_ref , gl_mark];
+ gl_meas = [gl_meas, gl_proj];
+ ij_meas = [ij_meas, ij_mark];
+end
+
+% calculate disparity
+gl_disp = gl_meas - gl_ref;
+
+%% plot
+quiver_scale = 10;
+figure(1);
+clf;
+quiver3sc( gl_ref(1,:) , gl_ref(2,:) , gl_ref(3,:) , ...
+ gl_disp(1,:), gl_disp(2,:), gl_disp(3,:), quiver_scale, 'k-');
+axis equal tight xy;
+
+xlabel('x / mm');
+ylabel('y / mm');
+zlabel('z / mm');
diff --git a/calibration/plot_traverse_plate_alignment.m b/calibration/plot_traverse_plate_alignment.m
new file mode 100644
index 0000000000000000000000000000000000000000..d883b5fe29d253f928e359c58d86c1a0e08111c9
--- /dev/null
+++ b/calibration/plot_traverse_plate_alignment.m
@@ -0,0 +1,60 @@
+%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
+% make a pretty plot for thesis of the traverse/plate alignment
+addpath([pwd '/../misc/']);
+trav_x = [-30
+-25
+-20
+-15
+-10
+-5
+0
+5
+10
+15
+20
+25
+30];
+trav_z1 = [-0.17
+-0.17
+-0.18
+-0.19
+-0.19
+-0.21
+-0.23
+-0.25
+-0.26
+-0.27
+-0.26
+-0.26
+-0.28];
+trav_z2 = [-0.2
+-0.19
+-0.16
+-0.14
+-0.15
+-0.16
+-0.16
+-0.17
+-0.18
+-0.17
+-0.16
+-0.15
+-0.16];
+trav_z = (trav_z1 + trav_z2)/2;
+p = polyfit(trav_x, trav_z, 1);
+
+figure(1);
+clf;
+plot( trav_x, trav_z1, 'ko', ...
+ trav_x, trav_z2, 'kx', ...
+ trav_x, polyval(p,trav_x),'k-');
+L = legend('Repeat 1', 'Repeat 2', 'Fit', 'Location', 'North');
+set(L, 'interpreter', 'latex', 'fontsize', 20);
+xlim([-30 30]);
+ylim([-1 1]);
+set(gca, 'ytick', -1:0.2:1);
+grid on;
+daspect([15 1 1]);
+
+make_plot_pretty('$x$ position / mm', ...
+ '$z$ position / mm', '', 20);
diff --git a/calibration/poly/polyfit2d3.m b/calibration/poly/polyfit2d3.m
new file mode 100644
index 0000000000000000000000000000000000000000..fcd9c9e8f22e3b24957dcf9167dd703de9f5e6f9
--- /dev/null
+++ b/calibration/poly/polyfit2d3.m
@@ -0,0 +1,26 @@
+function P = polyfit2d3(u, uprime)
+ % P = polyfit2d(u, uprime)
+ % find best fit coefficients P for the polynomial map u' = f(u; P)
+ %
+ % f(u) is a 2D, 3rd order polynomial, defined in terms of 20 polynomial
+ % coefficients P_ij, 10 for each component
+ %
+ % f_0(x,y) = P_00 + P_01 x + P_13 y + P_04 x^2 + P_05 xy + P_06 y^2 + P_07 x^3 + P_08 x^2y + P_09 xy^2 + P_09 y^3
+ % f_1(x,y) = P_10 + P_11 x + P_13 y + P_14 x^2 + P_15 xy + P_16 y^2 + P_17 x^3 + P_18 x^2y + P_19 xy^2 + P_19 y^3
+ %
+
+ %% extract parameters
+ N = size(u,2);
+ x = u(1,:);
+ y = u(2,:);
+ X = uprime(1,:);
+ Y = uprime(2,:);
+
+ %% least squares fit
+ T = [ones(1,N); x; y; x.^2; x.*y; y.^2; x.^3; x.*x.*y; x.*y.*y; y.^3].';
+ P0 = T \ X(:);
+ P1 = T \ Y(:);
+
+ P = [P0.'; P1.'];
+end
+
\ No newline at end of file
diff --git a/calibration/poly/polyfit2d4.m b/calibration/poly/polyfit2d4.m
new file mode 100644
index 0000000000000000000000000000000000000000..fb98e830fbe71eea82f7bcdd814a9fda668bb403
--- /dev/null
+++ b/calibration/poly/polyfit2d4.m
@@ -0,0 +1,30 @@
+function P = polyfit2d4(u, uprime)
+ % P = polyfit2d(u, uprime)
+ % find best fit coefficients P for the polynomial map u' = f(u; P)
+ %
+ % f(u) is a 2D, 3rd order polynomial, defined in terms of 20 polynomial
+ % coefficients P_ij, 10 for each component
+ %
+ % f_0(x,y) = P_00 + P_01 x + P_13 y + P_04 x^2 + P_05 xy + P_06 y^2 + P_07 x^3 + P_08 x^2y + P_09 xy^2 + P_09 y^3
+ % f_1(x,y) = P_10 + P_11 x + P_13 y + P_14 x^2 + P_15 xy + P_16 y^2 + P_17 x^3 + P_18 x^2y + P_19 xy^2 + P_19 y^3
+ %
+
+ %% extract parameters
+ N = size(u,2);
+ x = u(1,:);
+ y = u(2,:);
+ X = uprime(1,:);
+ Y = uprime(2,:);
+
+ %% least squares fit
+ T = [ones(1,N);
+ x; y;
+ x.^2; x.*y; y.^2; x.^3;
+ x.*x.*y; x.*y.*y; y.^3;
+ x.^4; x.^3.*y; x.^2.*y.^2; x.*y.^3; y.^4].';
+ P0 = T \ X(:);
+ P1 = T \ Y(:);
+
+ P = [P0.'; P1.'];
+end
+
\ No newline at end of file
diff --git a/calibration/poly/polymap2d3.m b/calibration/poly/polymap2d3.m
new file mode 100644
index 0000000000000000000000000000000000000000..3d1faf39d4c1c271196a5f5469bb82abcb99276f
--- /dev/null
+++ b/calibration/poly/polymap2d3.m
@@ -0,0 +1,31 @@
+function X = polymap2d3(P, u)
+ % X = polymap2d3(P, u)
+ %
+ % apply the 2D, 3rd order polynomial mapping X = f(x; P)
+ % to a set of points x, with polynomial coefficients P
+ %
+ % Input:
+ % x 2 x N array of coordinates
+ % P 2 x 10 array of polynomial coefficients
+ %
+ % Output:
+ % X 2 x N array of transformed coordinates
+ %
+ % The polynomial is a simple 2D, 3rd order mapping
+ %
+ % X_0 = P_00 + P_01 x + P_13 y + P_04 x^2 + P_05 xy + P_06 y^2 + P_07 x^3 + P_08 x^2y + P_09 xy^2 + P_09 y^3
+ % X_1 = P_10 + P_11 x + P_13 y + P_14 x^2 + P_15 xy + P_16 y^2 + P_17 x^3 + P_18 x^2y + P_19 xy^2 + P_19 y^3
+
+ N = size(u, 2);
+
+ x = u(1,:);
+ y = u(2,:);
+ X = P(1,1) + P(1,2)*x + P(1,3)*y ...
+ + P(1,4)*x.^2 + P(1,5)*x.*y + P(1,6)*y.^2 ...
+ + P(1,7)*x.^3 + P(1,8)*x.^2.*y + P(1,9)*x.*y.^2 + P(1,10)*y.^3;
+ Y = P(2,1) + P(2,2)*x + P(2,3)*y ...
+ + P(2,4)*x.^2 + P(2,5)*x.*y + P(2,6)*y.^2 ...
+ + P(2,7)*x.^3 + P(2,8)*x.^2.*y + P(2,9)*x.*y.^2 + P(2,10)*y.^3;
+
+ X = [X; Y];
+end
\ No newline at end of file
diff --git a/calibration/poly/polymap2d4.m b/calibration/poly/polymap2d4.m
new file mode 100644
index 0000000000000000000000000000000000000000..c6f55387d89381591fff3f0e39eb6c4d728e8c8d
--- /dev/null
+++ b/calibration/poly/polymap2d4.m
@@ -0,0 +1,33 @@
+function X = polymap2d4(P, u)
+ % X = polymap2d3(P, u)
+ %
+ % apply the 2D, 3rd order polynomial mapping X = f(x; P)
+ % to a set of points x, with polynomial coefficients P
+ %
+ % Input:
+ % x 2 x N array of coordinates
+ % P 2 x 10 array of polynomial coefficients
+ %
+ % Output:
+ % X 2 x N array of transformed coordinates
+ %
+ % The polynomial is a simple 2D, 3rd order mapping
+ %
+ % X_0 = P_00 + P_01 x + P_13 y + P_04 x^2 + P_05 xy + P_06 y^2 + P_07 x^3 + P_08 x^2y + P_09 xy^2 + P_09 y^3
+ % X_1 = P_10 + P_11 x + P_13 y + P_14 x^2 + P_15 xy + P_16 y^2 + P_17 x^3 + P_18 x^2y + P_19 xy^2 + P_19 y^3
+
+ N = size(u, 2);
+
+ x = u(1,:);
+ y = u(2,:);
+ X = P(1,1) + P(1,2)*x + P(1,3)*y ...
+ + P(1,4)*x.^2 + P(1,5)*x.*y + P(1,6)*y.^2 ...
+ + P(1,7)*x.^3 + P(1,8)*x.^2.*y + P(1,9)*x.*y.^2 + P(1,10)*y.^3 ...
+ + P(1,11)*x.^4 + P(1,12)*x.^3.*y + P(1,13)*x.^2.*y.^2 + P(1,14)*x.*y.^3 + P(1,15)*y.^4;
+ Y = P(2,1) + P(2,2)*x + P(2,3)*y ...
+ + P(2,4)*x.^2 + P(2,5)*x.*y + P(2,6)*y.^2 ...
+ + P(2,7)*x.^3 + P(2,8)*x.^2.*y + P(2,9)*x.*y.^2 + P(2,10)*y.^3 ...
+ + P(2,11)*x.^4 + P(2,12)*x.^3.*y + P(2,13)*x.^2.*y.^2 + P(2,14)*x.*y.^3 + P(2,15)*y.^4;
+
+ X = [X; Y];
+end
\ No newline at end of file
diff --git a/calibration/refraction_correction.m b/calibration/refraction_correction.m
new file mode 100644
index 0000000000000000000000000000000000000000..97d770aad71962c9ba521f9dd4d9348a09277380
--- /dev/null
+++ b/calibration/refraction_correction.m
@@ -0,0 +1,61 @@
+%% calculate
+t_over_r = linspace(0, 0.1, 100);
+phi1 = linspace(0, 60 / 180*pi, 61);
+phi2 = linspace(0, 60 / 180*pi, 61);
+n1 = 1.00;
+n2 = 1.52;
+
+[phi1_mat, t_mat] = ndgrid(phi1, t_over_r);
+sin_phi1 = sin(phi1_mat);
+tan_phi1 = tan(phi1_mat);
+sin_phi2 = n1/n2 * sin_phi1;
+phi2_mat = asin(sin_phi2);
+tan_phi2 = tan(phi2_mat);
+
+tan_ratio = @(phi1) n1 / n2 * sqrt((1 - sin(phi1).^2) ./ (1 - n1^2/n2^2*sin(phi1).^2));
+
+dilation = (1 + t_mat) ./ (1 + t_mat .* tan_ratio(phi1_mat));
+%dilation(1,:) = 1;
+
+%% plot
+figure(1);
+c_levels = 0 : 0.2 : 6;
+[c,h] = contour(phi1 * 180/pi, t_over_r, (dilation'-1)*100, c_levels, 'k-');
+clabel(c,h,'LabelSpacing',300);
+axis tight xy;
+
+xlabel('Camera angle $\Theta / ^\circ$', 'interpreter', 'latex', 'fontsize', 20);
+ylabel('Thickness $t/r$', 'interpreter', 'latex', 'fontsize', 20);
+ylim(t_over_r([1 end]));
+xlim(phi1([1 end])*180/pi);
+titstr = sprintf('Percentage dilation $100(\\Delta'' / \\Delta - 1)$ for $n_1 = %0.2f$ $n_2 = %0.2f$', n1, n2);
+title(titstr, 'interpreter', 'latex', 'fontsize', 20);
+
+set(gca, 'clim', c_levels([1 end]));
+
+%% calculate
+t_over_r = 0.01;
+[phi1_mat, phi2_mat] = ndgrid(phi1, phi2);
+dil21 = (1 + t_over_r * tan_ratio(phi2_mat)) ./ (1 + t_over_r * tan_ratio(phi1_mat));
+
+%% plot
+figure(2);
+clf;
+
+c_levels = -1 : 0.02 : 1;
+[c,h] = contour(phi1 * 180/pi, phi2 * 180/pi, (dil21'-1)*100, c_levels, 'k-');
+clabel(c,h,'LabelSpacing',300);
+axis equal tight xy;
+
+hold on;
+for fov = -20 : 5 : 20
+ plot(phi1 / pi*180, phi1/pi*180 + fov, 'r--');
+end
+
+xlabel('$\Theta_1 / ^\circ$', 'interpreter', 'latex', 'fontsize', 20);
+ylabel('$\Theta_2 / ^\circ$', 'interpreter', 'latex', 'fontsize', 20);
+
+xlim(phi1([1 end])*180/pi);
+ylim(phi2([1 end])*180/pi);
+titstr = sprintf('Relative dilation $100(\\Delta_2''/\\Delta_1'' - 1)$ for $n_1 = %0.2f$ $n_2 = %0.2f$, $t/r = %0.2f$', n1, n2, t_over_r);
+title(titstr, 'interpreter', 'latex', 'fontsize', 20);
diff --git a/calibration/search_markers.m b/calibration/search_markers.m
new file mode 100644
index 0000000000000000000000000000000000000000..0ff168c6c328795a460fdb4e2a60a3527d295bf1
--- /dev/null
+++ b/calibration/search_markers.m
@@ -0,0 +1,46 @@
+function [xy_match, ij_match, ij_pred] = search_markers(xy_pred, ij_markers, thresh, proj_type, varargin)
+ % [xy_match, ij_match] = search_markers(xy_pred, ij_markers, proj_type, varargin)
+ %
+ % Given a list of markers in image space ij_markers, search for
+ % those in this set that best correspond to grid locations xy_pred
+ % and are within a given distance thresh (in image space)
+ %
+ %
+ % Grid locations in xy space are mapped onto image space using either:
+ % * proj_type = 'affine'
+ % an affine transformation determined from the coordinates
+ % of the center, top and right markers (ij_center,ij_top,ij_right)
+ % * proj_type = 'pinhole'
+ % a 2D pinhole model P2D
+ %
+
+ %% project prediction of marker coord into image space
+ n_pred = size(xy_pred, 2);
+ if strcmpi(proj_type, 'affine')
+ % affine transformation
+ A = varargin{1};
+ b = varargin{2};
+ ij_pred = bsxfun(@plus, A * xy_pred, b);
+ elseif strcmpi(proj_type, 'pinhole')
+ % 2D pinhole model
+ P2D = varargin{1};
+ ij_pred = p2d_map_obj2im(P2D, xy_pred);
+ end
+
+ %% search for nearest matching marker
+ xy_match = xy_pred;
+ ij_match = zeros(2, n_pred);
+ d_match = zeros(1, n_pred);
+ for n = 1 : n_pred
+ dist = sqrt(sum(bsxfun(@minus, ij_markers, ij_pred(:, n)).^2, 1));
+ [mindist,imin] = min(dist);
+ ij_match(:,n) = ij_markers(:, imin);
+ d_match(n) = mindist;
+ end
+
+ %% eliminate those outside threshold
+ ok = d_match < thresh;
+ xy_match = xy_match(:, ok);
+ ij_match = ij_match(:, ok);
+ ij_pred = ij_pred(:, ok);
+end
diff --git a/calibration/sheet_calibration.m b/calibration/sheet_calibration.m
new file mode 100644
index 0000000000000000000000000000000000000000..933ba8f55c4d8208e4089dcdbadccd3b8aec140a
--- /dev/null
+++ b/calibration/sheet_calibration.m
@@ -0,0 +1,280 @@
+%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
+%
+% sheet_calibration.m
+%
+% calibration using bundle method to determine the position and thickness
+% of laser sheets, from images of laser sheets projected onto a calibration
+% target
+
+%% input parameters
+
+num_sheets = 50; % number of laser sheets
+
+calib_dir = 'D:/bigtank-jul-2013/CAL-20-07-13/NEWSHEET/';
+cam_model_file = [calib_dir '/camera-model-280415.mat'];
+cam_idx = 1;
+calib_file_pat = [calib_dir '/C-CAM1-%02d.mat'];
+sheet_img_file_pat= [calib_dir '/../SHEET-%02d/CAM1-volume-01-frame-%02d.tif'];
+sheet_model_file = [calib_dir '/sheet-model-280415.mat'];
+
+calib_file_range = 1 : 10;
+num_set_points = length(calib_file_range);
+
+% additional tweaking stuff
+max_thickness = 10; % max sheet thickness
+v_min = -60; % min v coord for line fit
+v_max = 60; % max v coord for line fit
+plate_du = 10; % distance between dots in u direction
+plate_dv = 10; % and in v direction
+
+DO_DEBUG_PLOT = false;
+
+%% load camera model and calibration setup
+calibration_setup = importdata(cam_model_file);
+gl_plate_pos = calibration_setup.gl_plate_pos;
+pl_to_gl = calibration_setup.pl_to_gl;
+camera_setup = calibration_setup.camera(cam_idx);
+camera_calib = calibration_setup.camera(cam_idx).calib;
+i_axis = camera_setup.i_axis;
+j_axis = camera_setup.j_axis;
+Ni = length(i_axis);
+Nj = length(j_axis);
+
+%% find intersections with laser sheet
+
+fprintf('------------------------------------------------------------------\n');
+fprintf('LASER SHEET CALIBRATION:\n');
+
+% matrix which converts from plate to global coordinate system
+% [x;y;z] = pl_to_gl * [u;v;w] + [xp; yp; zp]
+plate_eu = pl_to_gl(:,1);
+plate_ev = pl_to_gl(:,2);
+plate_ew = pl_to_gl(:,3);
+
+m_mat = nan(num_sheets, num_set_points); % m = dot(ev,er) / dot(eu, er)
+c_mat = nan(num_sheets, num_set_points); % c is the u coordinate at v = 0 along the line of intersection
+k_mat = nan(num_sheets, num_set_points); % k = dot(ex,er) / dot(ez, er)
+w_vec = zeros(1, num_sheets); % w is e^-2 sheet width
+w_mat = nan(num_sheets, num_set_points);
+
+gl_eZ_mat = zeros(num_sheets, 3); % sheet normal vector in global c sys
+gl_xs_mat = zeros(num_sheets, 3); % sheet origin in global c sys
+gl_on_sheet = nan(num_sheets, 10000, 3);
+
+% prepare figure
+figure(1);
+clf;
+
+for sheet_idx = 1:num_sheets
+ %% report
+ fprintf('Calculating plane of sheet %03d of %03d\n', sheet_idx, num_sheets);
+
+ %% find intersection for each set point
+ Nrows = 12;
+ w_proj_vec = nan(1, num_set_points);
+ gl_on_this_sheet = nan(3, Nrows, num_set_points);
+
+ for set_point = 1 : num_set_points
+ %% load plate image
+ calib_file_idx = calib_file_range(set_point);
+ image_fname = sprintf(sheet_img_file_pat, calib_file_idx, sheet_idx);
+ sheet_img_raw = double( fliplr(imread(image_fname)') );
+
+ %% get u,v axes from original calibration
+ calib_fname = sprintf(calib_file_pat, calib_file_idx);
+ original_calib = importdata(calib_fname);
+ u = original_calib.x_axis;
+ v = original_calib.y_axis;
+
+ %% dewarp
+ plate_pos = gl_plate_pos(:,set_point);
+ [umat,vmat] = ndgrid(u,v);
+ gl_px_mat = bsxfun(@plus, plate_pos, pl_to_gl*[umat(:) vmat(:) zeros(Ni*Nj,1)]');
+ ij_px_mat = pinhole_model(gl_px_mat, camera_calib);
+ imat = reshape(ij_px_mat(1,:), [Ni Nj]);
+ jmat = reshape(ij_px_mat(2,:), [Ni Nj]);
+ sheet_img = interp2(i_axis, j_axis, sheet_img_raw', imat, jmat);
+
+ %% find best fit of intersection to line u = mv + c
+ [line_coeffs, w_proj, line_u, line_v] = find_sheet_width(u, v, sheet_img, [v_min v_max], [plate_du plate_dv]);
+ m = line_coeffs(1);
+ c = line_coeffs(2);
+ bFound = true;
+ if isnan(w_proj)
+ bFound = false;
+ end
+
+ if bFound
+ % work out object space coords of points on sheet
+ % save them to matrix
+ gl_plate_origin = [plate_pos(1), 0, plate_pos(2)]';
+ pl_on_sheet = [line_u(:) line_v(:) zeros(Nrows,1)]';
+ gl_on_this_sheet(:,:,set_point) = (pl_to_gl * pl_on_sheet) + gl_plate_origin*ones(1,Nrows);
+ % save basic results of fit
+ m_mat(sheet_idx, set_point)= m;
+ c_mat(sheet_idx, set_point)= c;
+ w_proj_vec(set_point) = w_proj;
+ w_mat(sheet_idx, set_point)= w_proj;
+ end
+ %% plot
+ if DO_DEBUG_PLOT
+ figure(11);
+ clf;
+ row = 4;
+ %subplot1(1,2,'Gap',[0.06 0.05]);
+ %subplot1(1);
+
+ %% image of laser sheet
+ imagesc(u,v,sheet_img');
+ colormap gray;
+ clim([0 1024]);
+ axis equal tight xy;
+ hold on;
+ plot(polyval(line_coeffs, v([1 end])), v([1 end]), 'r-');
+ plot(line_u, line_v, 'ro');
+ plot(u([1 end]), line_v(row)*[1 1], 'w:');
+ make_plot_pretty('u / mm', 'v / mm', '', 20);
+
+ %% profile of laser sheet at different u
+ %subplot1(2);
+ figure(12);
+ clf;
+ hold on;
+ fit_object = fittype('A * exp(-8*(x-x0)^2/w^2)', 'coefficients', {'A','x0','w'}, 'independent', 'x');
+
+ [~,jj] = min(abs(v - line_v(row)));
+ in_range = abs(u - line_u(row)) < w_proj/2;
+ row_u = u(in_range);
+ row_profile = sheet_img(in_range, jj);
+ amp = max(row_profile);
+ fm = fit(row_u(:), row_profile(:), fit_object, 'startpoint', [amp, line_u(row), w_proj]);
+ plot(u, sheet_img(:,jj), 'k.');
+ plot(u, fm(u), 'r-', 'linewidth', 2);
+ plot(row_u(1 )*[1 1], [0 700], 'k:', ...
+ row_u(end)*[1 1], [0 700], 'k:');
+
+ axis square;
+ make_plot_pretty('u / mm', 'Intensity / counts', '', 20);
+ xlim(round(line_u(1) + [-10 10]));
+ ylim([0 700]);
+ set(gca,'ytick', 0 : 100 : 700);
+ legend('Measurement', 'Fit', 'Fit range');
+ end
+ end
+
+ %% find eZ, the sheet normal vector
+ % use a least squares fit to the equation of plane
+ % c = dot(x_p, gl_eZ)
+ Np = size(gl_on_this_sheet,2) * size(gl_on_this_sheet,3);
+ gl_on_this_sheet = reshape(gl_on_this_sheet, [3 Np]);
+ % repeatedly apply fit, excluding outliers
+ gl_on_sheet_filt = gl_on_this_sheet(:, ~isnan(gl_on_this_sheet(1,:)));
+ for fit_iter = 1:3
+ % apply fit
+ Np = size(gl_on_sheet_filt, 2);
+ [M,~,res] = lsqlin(gl_on_sheet_filt', ones(Np,1), [], []);
+ % identify outliers
+ res = res.^2;
+ res_thresh = prctile(res, 95);
+ keep = res < res_thresh;
+ gl_on_sheet_filt = gl_on_sheet_filt(:, keep);
+ end
+ % fit solves M(1)*x + M(2)*y + M(3)*z = 1 for the best M
+ gl_eZ = M / norm(M);
+ gl_eZ = gl_eZ * sign(gl_eZ(1));
+ gl_pos = M / norm(M)^2;
+
+ gl_xs_mat(sheet_idx, :) = gl_pos;
+ gl_eZ_mat(sheet_idx, :) = gl_eZ;
+
+ Np = size(gl_on_sheet_filt, 2);
+ gl_on_sheet(sheet_idx, 1:Np, :) = gl_on_sheet_filt';
+
+ %% find sheet width
+ sin_theta = sqrt(1 - dot(gl_eZ, plate_ew)^2);
+ esq_width = w_mat(sheet_idx, :);
+ esq_width = mean(esq_width(~isnan(esq_width)));
+ esq_width = esq_width * sin_theta;
+ w_vec(sheet_idx) = esq_width;
+
+ %% report
+ fprintf(' e_r = [%0.3f %0.3f %0.3f]\n', gl_eZ(1), gl_eZ(2), gl_eZ(3));
+ fprintf(' x_p = [%0.3f %0.3f %0.3f]\n', gl_pos(1), gl_pos(2), gl_pos(3));
+ fprintf(' w = %0.3f mm\n', w_vec(sheet_idx));
+
+ %% plot points
+ figure(1);
+ %plot3(gl_on_sheet(1,:), gl_on_sheet(2,:), gl_on_sheet(3,:), 'k.');
+ hold on;
+ plot3(gl_on_sheet_filt(1,:), gl_on_sheet_filt(2,:), gl_on_sheet_filt(3,:), 'r.');
+
+ gl_pop_x = [-1 -1 +1 +1]*25 - 5;
+ gl_pop_y = [-1 +1 +1 -1]*60;
+ gl_pop_z = (dot(gl_pos,gl_eZ) - gl_eZ(1)*gl_pop_x - gl_eZ(2)*gl_pop_y) / gl_eZ(3);
+ S = fill3(gl_pop_x, gl_pop_y, gl_pop_z, 'green');
+ set(S, 'FaceAlpha', 0.15, 'EdgeAlpha', 0);
+end
+
+gl_on_sheet = reshape(gl_on_sheet, [numel(gl_on_sheet)/3 3]);
+keep = ~isnan(gl_on_sheet(:,1));
+gl_on_sheet = gl_on_sheet(keep, :)';
+
+%% pretty plot of markers
+figure(1);
+clf;
+% draw points on sheet
+plot3(gl_on_sheet(1,:), gl_on_sheet(2,:), gl_on_sheet(3,:), 'r.');
+hold on;
+% draw sheets
+S_patches = zeros(num_sheets,1);
+for k = 1 : num_sheets
+ gl_eZ = gl_eZ_mat(k,:);
+ gl_pos = gl_xs_mat(k,:);
+ gl_pop_x = [-1 -1 +1 +1]*25 - 5;
+ gl_pop_y = [-1 +1 +1 -1]*60;
+ gl_pop_z = (dot(gl_pos,gl_eZ) - gl_eZ(1)*gl_pop_x - gl_eZ(2)*gl_pop_y) / gl_eZ(3);
+ S_patches(k) = fill3(gl_pop_x, gl_pop_y, gl_pop_z, 'green');
+end
+set(S_patches, 'EdgeColor', 'none', 'FaceAlpha', 0.10);
+
+%figure(1);
+xlabel('x');
+ylabel('y');
+zlabel('z');
+axis equal tight xy;
+
+%% find sheet spacing
+
+gl_eZ = mean(gl_eZ_mat, 1)';
+esq_width = mean(w_vec);
+sheet_dist = gl_xs_mat * gl_eZ;
+scan_coeffs = polyfit(1:num_sheets, sheet_dist', 1);
+scan_dz = abs(scan_coeffs(1));
+position_error = (sheet_dist' - polyval(scan_coeffs, 1:num_sheets));
+std_position_error = std(position_error);
+
+figure(2);
+plot(1 : num_sheets, sheet_dist, 'b.', ...
+ 1 : num_sheets, polyval(scan_coeffs,1:num_sheets), 'k-');
+xlabel('Sheet index');
+ylabel('Sheet distance / mm');
+
+%% report
+
+fprintf('\n');
+fprintf('Mean sheet spacing: %0.2f mm\n', scan_dz);
+fprintf('RMS error in sheet position: %0.4f mm\n', std_position_error);
+fprintf('RMS error as percentage of dx: %0.2f %%\n', std_position_error / scan_dz * 100);
+fprintf('Mean sheet thickness: %0.2f mm\n', esq_width);
+fprintf('w / dz %0.2f\n', esq_width / scan_dz);
+
+%% save to file
+fprintf(' - Saving calibration to file ... ');
+fstruct = struct('n_sheets', num_sheets, ...
+ 'plate_eu', plate_eu, 'plate_ev', plate_ev, 'plate_ew', plate_ew, ...
+ 'gl_sheet_normal', gl_eZ_mat, ...
+ 'gl_sheet_pos', gl_xs_mat, ...
+ 'sheet_width', w_vec, ...
+ 'gl_on_sheet', gl_on_sheet);
+save(sheet_model_file, '-struct', 'fstruct');
+fprintf('done\n');
\ No newline at end of file
diff --git a/calibration/test_bounding_polygon.m b/calibration/test_bounding_polygon.m
new file mode 100644
index 0000000000000000000000000000000000000000..5a05f493c502d4ab82feb5b94bdfadbb18ab7ba3
--- /dev/null
+++ b/calibration/test_bounding_polygon.m
@@ -0,0 +1,19 @@
+x = rand(25,1);
+y = rand(25,1);
+idx = convhull(x,y);
+corners = [x(idx), y(idx)]';
+
+[A,b] = bounding_polygon(corners);
+
+xx = rand(1000,1);
+yy = rand(1000,1);
+pts = [xx,yy]';
+b_inside = all(bsxfun(@minus, A*pts, b) <= 0, 1);
+
+figure(1);
+clf;
+%plot(x,y,'k.');
+hold on;
+plot(corners(1,:), corners(2,:), 'ro-');
+plot(xx(b_inside), yy(b_inside), 'r.');
+plot(xx(~b_inside), yy(~b_inside), 'k.');
diff --git a/calibration/test_pinhole.m b/calibration/test_pinhole.m
new file mode 100644
index 0000000000000000000000000000000000000000..a7c4f7bef9b14156fd36df00494cf096a1777119
--- /dev/null
+++ b/calibration/test_pinhole.m
@@ -0,0 +1,47 @@
+%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
+% test computation of Jacobian in pinhole model
+
+%% input parameters
+calib_dir = '/nfs/data.lfpn/lawson/160412/calib/';
+calib_file = 'camera-model-160418-0000-0099.mat';
+n_pos = 1000;
+cam_idx = 3;
+
+dx = rand(3,1);
+dx = dx / norm(dx);
+dx = 0.2 * dx;
+
+%% load calibration
+fname = [calib_dir '/' calib_file];
+setup = importdata(fname);
+camera = setup.camera;
+n_cameras = setup.n_cameras;
+r_sphere = setup.r_sphere_lim;
+gl_origin = setup.gl_origin;
+
+calib = camera(cam_idx).calib;
+
+%% generate random points in sphere
+gl_ctr = r_sphere * (0.5 - rand(3,n_pos));
+gl_shift = bsxfun(@plus, gl_ctr, dx);
+gl_dx = gl_shift - gl_ctr;
+
+[ij_ctr, jac] = pinhole_model(gl_ctr, calib, true);
+ij_shift = pinhole_model(gl_shift, calib, true);
+ij_est = ij_ctr + squeeze(mtimesx(jac, reshape(gl_dx, [3 1 n_pos])));
+
+ij_res = ij_est - ij_shift;
+ij_res_rms = rms(ij_res,2);
+ij_mov = ij_shift - ij_ctr;
+ij_mov_rms = rms(ij_mov,2);
+
+fprintf('RMS displacment: %0.3f %0.3f px \n', ij_mov_rms);
+fprintf('Error of estimator %0.3f %0.3f px\n', ij_res_rms);
+
+%% plot
+figure(1);
+clf;
+plot(ij_shift(1,:), ij_shift(2,:), 'k.');
+hold on
+plot(ij_ctr(1,:), ij_ctr(2,:), 'r.');
+plot(ij_est(1,:), ij_est(2,:), 'ko');
\ No newline at end of file
diff --git a/calibration/volume_bounds.m b/calibration/volume_bounds.m
new file mode 100644
index 0000000000000000000000000000000000000000..46da91b8af5b533167f74b414165ece5eaf029de
--- /dev/null
+++ b/calibration/volume_bounds.m
@@ -0,0 +1,30 @@
+function [gl_origin, r_sphere] = volume_bounds(setup)
+ % gl_origin, r_sphere = volume_bounds(setup)
+ %
+ % determine the largest sphere which is visible in each camera
+ % of the camera setup specified by setup
+ %
+ % returns the origin and radius of the sphere in each view
+
+ %% determine size + center of volume
+ % find point closest to center of each image in all cameras
+ n_cameras = setup.n_cameras;
+ ij_origin = zeros(2,1,n_cameras);
+ for cam_idx = 1 : n_cameras
+ i_axis = setup.camera(cam_idx).i_axis;
+ j_axis = setup.camera(cam_idx).j_axis;
+ ij_origin(:,1,cam_idx)= [sum(i_axis([1 end]))/2;
+ sum(j_axis([1 end]))/2];
+ end
+ gl_origin = triangulate(setup.camera, ij_origin, true);
+
+ % find size of inscribed sphere
+ r_sphere = zeros(1, n_cameras);
+ for c = 1 : n_cameras
+ M = frustum(setup.camera(c));
+ frustum_dist = M * [gl_origin(:);1];
+ r_sphere(c) = min(abs(frustum_dist));
+ end
+end
+
+
diff --git a/stereo/call_spiv_selfcal.m b/stereo/call_spiv_selfcal.m
new file mode 100644
index 0000000000000000000000000000000000000000..0a87119acab3071bb7a3e374167721051447b612
--- /dev/null
+++ b/stereo/call_spiv_selfcal.m
@@ -0,0 +1,8 @@
+function ws = call_spiv_selfcal(opts)
+ %% extract user defined options
+ src_dir = opts.src_dir;
+ spiv_selfcal;
+
+ %% save workspace
+ saveas('test.mat');
+end
\ No newline at end of file
diff --git a/stereo/define_planar_coord_system.m b/stereo/define_planar_coord_system.m
new file mode 100644
index 0000000000000000000000000000000000000000..80d0606da6e7ca27cbe50f58580aad1284d0017e
--- /dev/null
+++ b/stereo/define_planar_coord_system.m
@@ -0,0 +1,26 @@
+function [pl_to_re, x_0] = define_planar_coord_system(M, e_up)
+ % pl_to_re, x_0 = define_planar_coord_system(M, e_up)
+ % define a coordinate system for the plane specified by
+ % M*[x; 1] = 0
+ % such that the resulting coordinate system
+ % x = x_0 + u*e_u + v*e_v + w*e_w
+ % corresponds to
+ % w = 0 on the plane
+ % and
+ % e_v is aligned with e_up
+
+ e_up = e_up(:);
+ M = reshape(M, [1 4]);
+ M(1:3) = M(1:3) / norm(M(1:3));
+
+
+ e_w = M(1:3)';
+ % "up" vector, usually aligned with y
+ e_v = e_up - dot(e_w,e_up)*e_w;
+ e_v = e_v / norm(e_v);
+ % "right" vector, orthogonal to up and normal
+ e_u = cross(e_v, e_w);
+ % origin
+ x_0 = -e_w * M(4);
+ pl_to_re = [e_u(:) e_v(:) e_w(:)];
+end
\ No newline at end of file
diff --git a/stereo/fit_plane.m b/stereo/fit_plane.m
new file mode 100644
index 0000000000000000000000000000000000000000..ef2a4c28853f2e4c7cd201799317a637012a687d
--- /dev/null
+++ b/stereo/fit_plane.m
@@ -0,0 +1,18 @@
+function M = fit_plane(x, e_u)
+ % M = fit_plane(x, e_u)
+ % fit a set of points x = [3 x N] to a plane defined by
+ % M * [x;1] = 0
+ %
+ % this problem is uniquely specified up to the orientation of
+ % the unit normal (provided x do not lie on a line or point)
+ %
+ % the orientation of the unit normal can be chosen so it is aligned
+ % with a unit vector e_u, in the sense that n.e_u > 0
+ %
+
+ [M,~] = svds([x; ones(1,size(x,2))], 1, 'smallest');
+ M = M' / norm(M(1:3));
+ if nargin > 1
+ M = M * sign(dot(M(1:3), e_u));
+ end
+end
\ No newline at end of file
diff --git a/stereo/fwhm.m b/stereo/fwhm.m
new file mode 100644
index 0000000000000000000000000000000000000000..8f045cce0e4f17bea2f814c83b38c8f47850fdff
--- /dev/null
+++ b/stereo/fwhm.m
@@ -0,0 +1,9 @@
+function W = fwhm(x, f)
+ % W = fwhm(x, f)
+ % measure full width half maximum of f(x)
+ f = f - min(f);
+ pk = max(f);
+ i0 = find(f > 0.5*pk, 1, 'first');
+ i1 = find(f > 0.5*pk, 1, 'last');
+ W = x(i1) - x(i0);
+end
\ No newline at end of file
diff --git a/stereo/gauss2d.m b/stereo/gauss2d.m
new file mode 100644
index 0000000000000000000000000000000000000000..59257b641e4f3d58b8505c012889d3378118779e
--- /dev/null
+++ b/stereo/gauss2d.m
@@ -0,0 +1,39 @@
+function [f, jac] = gauss2d(X, xdata)
+ % [f,jac] = gauss2d(X, xdata)
+ %
+ % Gaussian fit model function
+ % f = A * exp(-(x-x0)^2/sx^2 - (y-y0)^2/sy^2)
+ % with jacobian
+ % df/dX_i
+ %
+ % where the model is specified by
+ % X(1) = A
+ % X(2) = x0
+ % X(3) = y0
+ % X(4) = sx
+ % X(5) = sy
+ %
+ % and x = xdata(:,1), y = xdata(:,2)
+
+ %% unpack
+ A = X(1);
+ x0 = X(2);
+ y0 = X(3);
+ sx = X(4);
+ sy = X(5);
+
+ %% evaluate
+ x = xdata(:,1);
+ y = xdata(:,2);
+ f = A * exp(-(x-x0).^2/sx^2 - (y-y0).^2/sy^2);
+
+ %% jacobian
+ if nargout > 1
+ dfdA = f/A;
+ dfdx0 = 2*f.*(x-x0)/sx^2;
+ dfdy0 = 2*f.*(y-y0)/sy^2;
+ dfdsx = 2*f.*(x-x0).^2/sx^3;
+ dfdsy = 2*f.*(y-y0).^2/sy^3;
+ jac = [dfdA dfdx0 dfdy0 dfdsx dfdsy];
+ end
+end
\ No newline at end of file
diff --git a/stereo/gauss2d_peaklocate.m b/stereo/gauss2d_peaklocate.m
new file mode 100644
index 0000000000000000000000000000000000000000..d8eb70656392bbac080c4f4e2097fa99a8d7ce66
--- /dev/null
+++ b/stereo/gauss2d_peaklocate.m
@@ -0,0 +1,56 @@
+function [pk_loc, A, sx, sy, C_fit] = gauss2d_peaklocate(C)
+ % Gaussian 2D peak location using five parameter least-squares Gaussian
+ % fit
+
+ %% exclude points outside wsize/2 shift
+ S = size(C);
+ S2 = S;
+ %{
+ ir = (1+S(1)/4) : S(1)*3/4;
+ jr = (1+S(2)/4) : S(2)*3/4;
+ Cs = C(ir,jr);
+ %}
+ ir = 1 : S(1);
+ jr = 1 : S(2);
+ Cs = C;
+
+ %% find index of a strict global maximum
+ is_max = Cs(2:end-1,2:end-1) > Cs(1:end-2, 2:end-1) ...
+ & Cs(2:end-1,2:end-1) > Cs(3:end-0, 2:end-1) ...
+ & Cs(2:end-1,2:end-1) > Cs(2:end-1, 1:end-2) ...
+ & Cs(2:end-1,2:end-1) > Cs(2:end-1, 3:end-0);
+ is_max = Cs .* padarray(is_max, [1 1], 0, 'both');
+ [A, maxind] = max(is_max(:));
+ [imax,jmax] = ind2sub(S2, maxind);
+
+ %% handle corner case where correlation plane is negative valued
+ if A <= 0 || imax == 1 || imax == S(1) || jmax == 1 || jmax == S(2)
+ pk_loc = nan;
+ sx = nan;
+ sy = nan;
+ C_fit = nan*Cs;
+ return;
+ end
+
+ %% fit
+ [im,jm] = ndgrid(ir, jr);
+ xdata = [im(:) jm(:)];
+ ydata = Cs(:);
+ X0 = [A, imax+ir(1)-1, jmax+jr(1)-1, 4, 4];
+ X_lb = [0, S(1)/4, S(1)/4, 0 , 0];
+ X_ub = [inf,3*S(2)/4, 3*S(2)/4, S(1), S(2)];
+ fitopts = optimoptions('lsqcurvefit', 'display', 'off', 'SpecifyObjectiveGradient', false);
+ [X,~,~,flag,output] = lsqcurvefit(@gauss2d, X0, xdata, ydata, X_lb, X_ub, fitopts);
+ %disp(flag);
+ %disp(output);
+
+ %% residual
+ C_fit = reshape(gauss2d(X,xdata), size(Cs));
+ %C_fit = padarray(C_fit, S/4, 0, 'both');
+
+ %% extract
+ A = X(1);
+ pk_loc = [X(2);X(3)];
+ sx = X(4);
+ sy = X(5);
+end
\ No newline at end of file
diff --git a/stereo/is_local_maximum.m b/stereo/is_local_maximum.m
new file mode 100644
index 0000000000000000000000000000000000000000..c7942c5e48d45dbce6251f06de9cb24423e314c8
--- /dev/null
+++ b/stereo/is_local_maximum.m
@@ -0,0 +1,7 @@
+function b = is_local_maximum(im)
+ b = im(2:end-1,2:end-1) > im(1:end-2,2:end-1) ...
+ & im(2:end-1,2:end-1) > im(3:end ,2:end-1) ...
+ & im(2:end-1,2:end-1) > im(2:end-1,3:end ) ...
+ & im(2:end-1,2:end-1) > im(2:end-1,1:end-2);
+ b = padarray(b, [1 1], 0, 'both');
+end
\ No newline at end of file
diff --git a/stereo/loadvec_pair.m b/stereo/loadvec_pair.m
new file mode 100644
index 0000000000000000000000000000000000000000..7c164693bf25fbedd1f8612856e4efcc6bc2ff98
--- /dev/null
+++ b/stereo/loadvec_pair.m
@@ -0,0 +1,26 @@
+function [VC1, VC2] = loadvec_pair(vec_name)
+ % [VC1, VC2] = loadvec_pair(vec_name)
+ % load a pair of vector fields from either MATLAB or VC7 format
+ %
+
+ [~,~,ext] = fileparts(vec_name);
+ if strcmpi(ext, '.vc7')
+ % load vc7 vector field
+ F = readimx(vec_name);
+ VC1 = create2DVec(F.Frames{1});
+ VC2 = create2DVec(F.Frames{2});
+ % get PIV delta t
+ attrs = readAttributes(F);
+ dt = attrs('Reference time dt 1');
+ VC1.dt = dt;
+ VC2.dt = dt;
+ else
+ % load matlab vector field
+ % in same format as create2DVec
+ fs = load(vec_name);
+ [xm1, ym1] = ndgrid(fs.C1.win_x, fs.C1.win_y);
+ [xm2, ym2] = ndgrid(fs.C2.win_x, fs.C2.win_y);
+ VC1 = struct('X', xm1, 'Y', ym1, 'U', fs.C1.ux, 'V', fs.C1.uy, 'C', fs.C1.peak_choice, 'dt', 1);
+ VC2 = struct('X', xm2, 'Y', ym2, 'U', fs.C2.ux, 'V', fs.C2.uy, 'C', fs.C2.peak_choice, 'dt', 1);
+ end
+end
\ No newline at end of file
diff --git a/stereo/refine_marker.m b/stereo/refine_marker.m
new file mode 100644
index 0000000000000000000000000000000000000000..e4bd6716267b5e8131685200dc8e376e03466373
--- /dev/null
+++ b/stereo/refine_marker.m
@@ -0,0 +1,48 @@
+function pl_refined = refine_marker(u, v, im, pl_marker, radius)
+ % pl_refined = refine_marker(u_axis, v_axis, im, pl_marker, radius)
+ %
+ % refine the position measurement of circular markers on calibration
+ % image
+ %
+ % Input:
+ % u u coordinate axis
+ % v v coordinate axis
+ % im calibration image (dewarped)
+ % pl_marker 2 x N array of approximate marker locations
+ % radius nominal radius of markers
+
+ w_search = 6*radius;
+ n_markers = size(pl_marker,2);
+ du = mean(diff(u));
+ dv = mean(diff(v));
+ assert(abs(abs(du/dv) - 1) < 0.01, 'pixels must be square');
+ r_px = radius / sqrt(du*dv);
+
+
+ for m = 1 : n_markers
+ %% cut out region containing just one marker
+ ir = abs(u - pl_marker(1,m)) < w_search/2;
+ jr = abs(v - pl_marker(2,m)) < w_search/2;
+ im_samp = im(ir,jr);
+ bg = min(im_samp(:));
+ im_samp = im_samp - bg;
+ u_samp = u(ir);
+ v_samp = v(jr);
+
+ %% adaptively choose threshold
+ % based on reasoning that marker is brightest thing in view
+ % and that it occupies a known fraction of the cut out region
+ A_samp = abs(u_samp([1 end])*[-1;1] * v_samp([1 end])*[-1;1]);
+ thresh = prctile(im_samp(:), 100*(1 - (pi*(1.0*radius)^2)/A_samp));
+ im_filt = im_samp > thresh;
+ [C2,R2] = imfindcircles(im_filt', round(r_px*[0.8 1.2]), 'objectpolarity', 'bright');
+ [C1,R1] = imfindcircles(im_samp', round(r_px*[0.8 1.2]), 'objectpolarity', 'bright');
+
+ imagesc(im_samp');
+ viscircles(C2,R2,'color','r');
+ viscircles(C1,R1,'color','g');
+ axis equal tight xy;
+ end
+end
+
+
\ No newline at end of file
diff --git a/stereo/rescale_field.m b/stereo/rescale_field.m
new file mode 100644
index 0000000000000000000000000000000000000000..13213cb513d044cd05cc63ae1a3e7a0f21fd838d
--- /dev/null
+++ b/stereo/rescale_field.m
@@ -0,0 +1,4 @@
+function V = rescale_field(V, scale)
+ V.U = V.U * scale;
+ V.V = V.V * scale;
+end
\ No newline at end of file
diff --git a/stereo/robust_fit_plane.m b/stereo/robust_fit_plane.m
new file mode 100644
index 0000000000000000000000000000000000000000..c9c7b1cd489c51b5b21d1251f7b95393cc21bec3
--- /dev/null
+++ b/stereo/robust_fit_plane.m
@@ -0,0 +1,7 @@
+function M = robust_fit_plane(X)
+ xy = X(1:2,:)';
+ z = X(3,:)';
+ B = robustfit(xy, z);
+ M = [-B(2) -B(3) 1 -B(1)];
+ M = M / norm(M(1:3));
+end
\ No newline at end of file
diff --git a/stereo/roi_axes.m b/stereo/roi_axes.m
new file mode 100644
index 0000000000000000000000000000000000000000..17b3809d515758799efdd13c04e64da85ffe0618
--- /dev/null
+++ b/stereo/roi_axes.m
@@ -0,0 +1,71 @@
+function ax = roi_axes(cmodel, pl_corners, P)
+ % ax = roi_axes(c_A, c_B, re_corners, pl_corners, P)
+ %
+ % define a set of coordinate axes, based on an ROI defined by
+ % the corners
+ %
+ % Input:
+ % cmodel camera model array
+ % pl_corners corners of ROI on reconstruction plane
+ % P transformation matrix mapping reconstruction->world
+
+ %% get camera model and image axes
+ c_A = cmodel(1).calib;
+ c_B = cmodel(2).calib;
+ i_lim = [min(cmodel(1).i_axis) max(cmodel(1).i_axis)];
+ j_lim = [min(cmodel(1).j_axis) max(cmodel(1).j_axis)];
+
+ %% estimate a magnification
+ % based on matching reconstruction resolution to image resolution
+ re_corners = P * [pl_corners; 1 1 1 1];
+ scale = roi_magnification(c_A, c_B, re_corners, pl_corners);
+
+ %% define axes
+ pl_lim = [min(pl_corners,[],2) max(pl_corners,[],2)];
+ u_lim = pl_lim(1,:);
+ v_lim = pl_lim(2,:);
+ w_roi = abs(u_lim*[-1 1]');
+ h_roi = abs(v_lim*[-1 1]');
+ n_u = ceil(w_roi * scale(1));
+ n_v = ceil(h_roi * scale(2));
+ u_axis = linspace(u_lim(1), u_lim(2), n_u);
+ v_axis = linspace(v_lim(1), v_lim(2), n_v);
+
+ %% produce mapping function to image coordinates in both cameras
+ [um,vm,wm,tm] = ndgrid(u_axis, v_axis, 0, 1);
+ uvw1_mat = [um(:) vm(:) wm(:) tm(:)].';
+ xyz_mat = P * uvw1_mat;
+
+ [ijA, jacA_re] = pinhole_model(xyz_mat, c_A);
+ [ijB, jacB_re] = pinhole_model(xyz_mat, c_B);
+ iA = reshape(ijA(1,:), [n_u n_v]);
+ jA = reshape(ijA(2,:), [n_u n_v]);
+ iB = reshape(ijB(1,:), [n_u n_v]);
+ jB = reshape(ijB(2,:), [n_u n_v]);
+
+ % convert jacobian from world coordinates to sheet coordinates
+ jacA_pl = mtimesx(jacA_re, P(:,1:3));
+ jacB_pl = mtimesx(jacB_re, P(:,1:3));
+ % reshape to 2 x 3 x n_u x n_v
+ jacA_pl = reshape(jacA_pl, [2 3 n_u n_v]);
+ jacB_pl = reshape(jacB_pl, [2 3 n_u n_v]);
+
+ %% determine which points are out-of-bounds of image
+ imA_mask = iA < i_lim(1) | iA > i_lim(2) | jA < j_lim(1) | jA > j_lim(2);
+ imB_mask = iB < i_lim(1) | iB > i_lim(2) | jB < j_lim(1) | jB > j_lim(2);
+
+ %% create map structure
+ map = struct('im', iA, 'jm', jA, 'mask', imA_mask, 'jac', jacA_pl);
+ map(2) = struct('im', iB, 'jm', jB, 'mask', imB_mask, 'jac', jacB_pl);
+
+ %% save in struct
+ ax = struct( 'n_u', n_u, 'n_v', n_v, ...
+ 'u', u_axis, 'v', v_axis, ...
+ 'W', w_roi, 'H', h_roi, ...
+ 'scale', scale, ...
+ 'map', map, ...
+ 'cmodel', cmodel, ...
+ 'P', P, ...
+ 're_corners', re_corners, ...
+ 'pl_corners', pl_corners);
+end
\ No newline at end of file
diff --git a/stereo/roi_build_map.m b/stereo/roi_build_map.m
new file mode 100644
index 0000000000000000000000000000000000000000..20149eda3df6907f9249b91ec0b07ae9739cb3b0
--- /dev/null
+++ b/stereo/roi_build_map.m
@@ -0,0 +1,37 @@
+function roi = roi_build_map(roi)
+ %% grab parameters
+ u_axis = roi.u;
+ v_axis = roi.v;
+ P = roi.P;
+ c_A = cmodel(1).calib;
+ c_B = cmodel(2).calib;
+
+ %% produce mapping function to image coordinates in both cameras
+ [um,vm,wm,tm] = ndgrid(u_axis, v_axis, 0, 1);
+ uvw1_mat = [um(:) vm(:) wm(:) tm(:)].';
+ xyz_mat = P * uvw1_mat;
+
+ [ijA, jacA_re] = pinhole_model(xyz_mat, c_A);
+ [ijB, jacB_re] = pinhole_model(xyz_mat, c_B);
+ iA = reshape(ijA(1,:), [n_u n_v]);
+ jA = reshape(ijA(2,:), [n_u n_v]);
+ iB = reshape(ijB(1,:), [n_u n_v]);
+ jB = reshape(ijB(2,:), [n_u n_v]);
+
+ % convert jacobian from world coordinates to sheet coordinates
+ jacA_pl = mtimesx(jacA_re, P(:,1:3));
+ jacB_pl = mtimesx(jacB_re, P(:,1:3));
+ % reshape to 2 x 3 x n_u x n_v
+ jacA_pl = reshape(jacA_pl, [2 3 n_u n_v]);
+ jacB_pl = reshape(jacB_pl, [2 3 n_u n_v]);
+
+ %% determine which points are out-of-bounds of image
+ imA_mask = iA < i_lim(1) | iA > i_lim(2) | jA < j_lim(1) | jA > j_lim(2);
+ imB_mask = iB < i_lim(1) | iB > i_lim(2) | jB < j_lim(1) | jB > j_lim(2);
+
+ %% create map structure
+ map = struct('im', iA, 'jm', jA, 'mask', imA_mask, 'jac', jacA_pl);
+ map(2) = struct('im', iB, 'jm', jB, 'mask', imB_mask, 'jac', jacB_pl);
+
+ roi.map = map;
+end
\ No newline at end of file
diff --git a/stereo/roi_magnification.m b/stereo/roi_magnification.m
new file mode 100644
index 0000000000000000000000000000000000000000..cc7c680cc55a8fdd7a9514b52808ef45b5f94c26
--- /dev/null
+++ b/stereo/roi_magnification.m
@@ -0,0 +1,32 @@
+function scale = roi_magnification(c_A, c_B, re_corners, pl_corners)
+ % scale = roi_magnification(c_A, c_B, re_corners, pl_corners)
+ %
+ % estimate a magnification, based on matching reconstruction resolution
+ % to image resolution
+ %
+ % Input:
+ % c_A camera A calibration structure
+ % c_B camera B calibration structure
+ % re_corners corners of ROI in world coordinates
+ % pl_corners corners of ROI in reconstruction plane
+ % coordinates
+ %
+ % Output:
+ % scale approximate scale factor [px/mm] between image and
+ % reconstruction plane coordinates
+ %
+
+ %% project ROI corners onto image
+ im_corners_A = pinhole_model(re_corners, c_A);
+ im_corners_B = pinhole_model(re_corners, c_B);
+
+ %% determine affine transformation between world and image
+ U_inv = pinv([pl_corners(1:2,:); 1 1 1 1]);
+ Aff_A = im_corners_A * U_inv;
+ Aff_B = im_corners_B * U_inv;
+
+ %% average resolution across cameras
+ scale_A = eig(Aff_A(:,1:2));
+ scale_B = eig(Aff_B(:,1:2));
+ scale = sqrt(abs(scale_A .* scale_B));
+end
\ No newline at end of file
diff --git a/stereo/spiv_calibration.m b/stereo/spiv_calibration.m
new file mode 100644
index 0000000000000000000000000000000000000000..ebd7e96138df71e8705bf11cda2dee2b52730d72
--- /dev/null
+++ b/stereo/spiv_calibration.m
@@ -0,0 +1,355 @@
+addpath(genpath([pwd '/../']));
+
+%% input parameters
+calib_dir = 'H:\davis\multiscale\Properties\Calibration History\Gen1_123_CAL05_433.33\';
+marker_file = 'MarkPositionTable.xml'; % Calibration marker position file (usually 'MarkPositionTable.xml')
+model_file = 'refined.mat';
+
+b_square = true; % enforce square pixels?
+b_skew = false; % allow for skewness? (Scheimpflug adapters)
+b_distortion= true; % compensate for radial distortion?
+
+c_lim = [0 256];
+
+%% get marker coordinates
+% Retrieve calibration markers coordinate for both left and right camera
+calibname = [calib_dir marker_file];
+[Coord,corners_cam] = retrieve_marker_coord(calibname);
+i_axis = corners_cam(1,1) : corners_cam(1,2)-1;
+j_axis = corners_cam(2,1) : corners_cam(2,3)-1;
+
+%% load image
+% left image
+FA = readimx(sprintf('%s/camera1/B00001.im7', calib_dir));
+imA = FA.Frames{1}.Components{1}.Planes{1};
+if length(FA.Frames{1}.Components) > 1
+ imA_mask= FA.Frames{1}.Components{2}.Planes{1};
+else
+ imA_mask= zeros(size(imA));
+end
+
+% right image
+FB = readimx(sprintf('%s/camera2/B00001.im7', calib_dir));
+imB = FB.Frames{1}.Components{1}.Planes{1};
+if length(FB.Frames{1}.Components) > 1
+ imB_mask= FB.Frames{1}.Components{2}.Planes{1};
+else
+ imB_mask= zeros(size(imB));
+end
+
+% extract image and world coordinates
+im_mark_A = Coord.Left(:,1:2).';
+im_mark_B = Coord.Right(:,1:2).';
+re_mark_A = Coord.Left(:,3:5).';
+re_mark_B = Coord.Right(:,3:5).';
+n_A = size(im_mark_A,2);
+n_B = size(im_mark_B,2);
+
+% categorise markers on front/back planes
+re_mark = union(re_mark_A', re_mark_B', 'rows')';
+re_mark_front = re_mark(:, re_mark(3,:)>0);
+re_mark_back = re_mark(:, re_mark(3,:)<0);
+n_front = size(re_mark_front,2);
+n_back = size(re_mark_back, 2);
+r_nom = 0.75;
+
+%% fix depth
+warning('correcting for plate depth');
+correction = 1.0;% 25.4*40/1000 / 1.0;
+re_mark_A(3,:) = re_mark_A(3,:) * correction;
+re_mark_B(3,:) = re_mark_B(3,:) * correction;
+
+%% fit pinhole model
+fprintf('Fitting pinhole model ... \n');
+fitopts = struct('square', b_square, 'skew', b_skew, 'distortion', b_distortion);
+[pA,im_res_A] = pinhole_fit_3d_reg(im_mark_A, re_mark_A, fitopts);
+[pB,im_res_B] = pinhole_fit_3d_reg(im_mark_B, re_mark_B, fitopts);
+im_res_A_rms = rms(im_res_A,2);
+im_res_B_rms = rms(im_res_B,2);
+
+im_proj_A = pinhole_model(re_mark_A, pA, true);
+im_proj_B = pinhole_model(re_mark_B, pB, true);
+
+fprintf('Residual:\n');
+fprintf(' C1: %0.3f %0.3f px\n', im_res_A_rms);
+fprintf(' C2: %0.3f %0.3f px\n', im_res_B_rms);
+
+%% measure angle
+% angle is measured in x-z plane, relative to [0;0;1]
+theta1 = atand(-pA.xc(1) / pA.xc(3));
+theta2 = atand(+pB.xc(1) / pB.xc(3));
+fprintf('theta_1 = %6.2f deg\n', theta1);
+fprintf('theta_2 = %6.2f deg\n', theta2);
+
+%% plot
+figure(10);
+clf;
+plot3([pA.xc(1) 0], [pA.xc(2) 0], [pA.xc(3) 0], 'ro-');
+hold on;
+plot3([pB.xc(1) 0], [pB.xc(2) 0], [pB.xc(3) 0], 'go-');
+axis equal tight xy;
+view(0,180);
+
+%% save pinhole model
+fprintf('------------------------------------------------------------------\n');
+fprintf('Camera calibration complete\n');
+fprintf('Saving to file ... ');
+
+s_model = struct();
+s_model.pl_to_gl = eye(3);
+s_model.pl_to_gl_mat= repmat(eye(3),[1 1 2]);
+s_model.gl_plate_pos= [0 0 -0.5; 0 0 +0.5]';
+s_model.gl_origin = [0 0 0]';
+s_model.n_cameras = 2;
+s_model.n_views = 2;
+
+% camera calibration
+pA.im = imA;
+pB.im = imB;
+cA = struct('calib', pA, 'id', 1, 'i_axis', i_axis, 'j_axis', j_axis);
+cB = struct('calib', pB, 'id', 2, 'i_axis', i_axis, 'j_axis', j_axis);
+s_model.camera = [cA, cB];
+
+% bounding volume
+[~, r_sphere] = volume_bounds(s_model);
+r_sphere_lim = min(r_sphere);
+s_model.r_sphere = r_sphere;
+s_model.r_sphere_lim= r_sphere_lim;
+
+save([calib_dir model_file], '-struct', 's_model');
+
+fprintf('done\n');
+error('stop');
+%% dewarp images with pinhole model
+fprintf('Dewarping images ... \n');
+P_front = [s_model.pl_to_gl, [0;0;+0.5]];
+P_back = [s_model.pl_to_gl, [0;0;-0.5]];
+
+% front plane map
+fprintf(' front ... \n');
+front = StereoPlane(s_model.camera, P_front);
+AR = front.scale(2) / front.scale(1);
+[u_front,v_front]= front.make_axes([1 AR]);
+front.axes(u_front,v_front,false);
+
+pl_front_A = interpn(i_axis, j_axis, double(imA), front.map(1).im, front.map(1).jm, 'linear');
+pl_front_B = interpn(i_axis, j_axis, double(imB), front.map(2).im, front.map(2).jm, 'linear');
+pl_front_sum = imfuse(pl_front_A', pl_front_B');
+
+% back plane map
+fprintf(' back ... \n');
+back = StereoPlane(s_model.camera, P_back);
+AR = back.scale(2)/back.scale(1);
+[u_back,v_back] = back.make_axes([1 AR]);
+back.axes(u_back,v_back,false);
+
+pl_back_A = interpn(i_axis, j_axis, double(imA), back.map(1).im, back.map(1).jm, 'linear');
+pl_back_B = interpn(i_axis, j_axis, double(imB), back.map(2).im, back.map(2).jm, 'linear');
+pl_back_sum = imfuse(pl_back_A', pl_back_B');
+
+%% plot
+figure(1);
+clf;
+
+% left image with markers
+subplot(1,2,1);
+imagesc(i_axis,j_axis,imA');
+axis equal tight ij;
+xlim(i_axis([1 end]));
+ylim(j_axis([1 end]));
+hold on;
+plot(im_mark_A(1,:), im_mark_A(2,:), 'ro');
+plot(im_proj_A(1,:), im_proj_A(2,:), 'r.');
+quiver(im_proj_A(1,:), im_proj_A(2,:), im_res_A(1,:), im_res_A(2,:), 'r-');
+set(gca,'clim',c_lim);
+
+title('Camera 1');
+for n = 1 : n_A
+ str=sprintf('(%0.0f,%0.0f,%0.1f)', re_mark_A(:,n));
+ if re_mark_A(3,n) > 0
+ color = 'r';
+ else
+ color = 'g';
+ end
+ text(im_mark_A(1,n),im_mark_A(2,n)+64,str,'color',color);
+end
+
+
+% right image with markers
+subplot(1,2,2);
+imagesc(i_axis,j_axis,imB');
+axis equal tight ij;
+
+xlim(i_axis([1 end]));
+ylim(j_axis([1 end]));
+hold on;
+plot(im_mark_B(1,:), im_mark_B(2,:), 'ro');
+title('Camera 2');
+set(gca,'clim',c_lim);
+
+%% sum of camera images
+figure(2);
+clf;
+subplot(1,2,1);
+image(front.u, front.v, pl_front_sum);
+axis equal tight xy;
+title('Sum of cameras, front');
+ax = gca;
+
+% plot circles
+viscircles(ax(1), re_mark_front(1:2,:)' - [0.14 -0.16], r_nom*ones(n_front,1));
+
+subplot(1,2,2);
+image(back.u, back.v, pl_back_sum);
+axis equal tight xy;
+title('Sum of cameras, back');
+ax(2) = gca;
+
+viscircles(ax(2), re_mark_back(1:2,:)' - [0.20 -0.16], r_nom*ones(n_back,1));
+
+linkaxes(ax, 'xy');
+
+error('stop');
+
+%% refine marker location
+re_mark = {re_mark_A, re_mark_B};
+pl_front = cat(3, pl_front_A, pl_front_B);
+pl_back = cat(3, pl_back_A, pl_back_B);
+
+re_mark_refined = cell(1,2);
+im_mark_refined = cell(1,2);
+
+for c = 1 : 2
+ fprintf('refining markers in camera %d ...\n', c);
+
+ %% identify front/back planes
+ b_front = re_mark{c}(3, :) > 0;
+ b_back = ~b_front;
+ re_front = re_mark{c}(:, b_front);
+ re_back = re_mark{c}(:, b_back);
+ r_px = r_nom / mean(diff(front.u));
+ r_lim_px = round(r_px * [0.8 1.2]);
+ du = mean(diff(front.u));
+ dv = mean(diff(front.v));
+
+ %% refine markers
+ re_refined = refine_marker(front.u, front.v, pl_front(:,:,c), re_front, r_nom);
+
+ %% search for markers
+ sensitivity = 0.9;
+ [C_front, R_front] = imfindcircles(pl_front(:,:,c)', r_lim_px, 'Sensitivity', sensitivity);
+ [C_back , R_back ] = imfindcircles(pl_back(:,:,c)' , r_lim_px, 'Sensitivity', sensitivity);
+ % transform from coord sys
+ re_front_refined = [front.u(1); front.v(1); mean(re_front(3,:))] ...
+ + [du 0; 0 dv; 0 0] * C_front';
+ re_back_refined = [back.u(1); back.v(1); mean(re_back(3,:))] ...
+ + [du 0; 0 dv; 0 0] * C_back';
+ R_front = R_front*du;
+ R_back = R_back *du;
+
+ %% intersect with original definitions of markers
+ r_tol = r_nom*2;
+ % front plane
+ [idx, dist] = knnsearch(re_front_refined', re_front', 'K', 1);
+ b_ok = dist < r_tol;
+ re_front_refined = re_front_refined(:, idx(b_ok));
+ R_front = R_front(idx(b_ok));
+ % back plane
+ [idx, dist] = knnsearch(re_back_refined', re_back', 'K', 1);
+ b_ok = dist < r_tol;
+ re_back_refined = re_back_refined(:, idx(b_ok));
+ R_back = R_back(idx(b_ok));
+
+ %% reproject to images
+ im_front_refined = pinhole_model(re_front_refined, s_model.camera(c).calib, true);
+ im_back_refined = pinhole_model(re_back_refined , s_model.camera(c).calib, true);
+ pl_front_refined = [round(re_front_refined(1:2,:)/10)*10; re_front_refined(3,:)];
+ pl_back_refined = [round(re_back_refined(1:2,:)/10)*10; re_back_refined(3,:)];
+
+ %% store markers
+ re_mark_refined{c} = cat(2, pl_front_refined, pl_back_refined);
+ im_mark_refined{c} = cat(2, im_front_refined, im_back_refined);
+
+ %% plot
+ figure(10+c);
+ clf;
+ subplot(1,2,1);
+ imagesc(front.u,front.v,pl_front(:,:,c)');
+ axis equal tight xy;
+ viscircles(re_front_refined(1:2,:)', R_front);
+ title('front plane');
+ ax(1)=gca;
+ hold on;
+
+ subplot(1,2,2);
+ imagesc(back.u, back.v, pl_back(:,:,c)');
+ axis equal tight xy;
+ viscircles(re_back_refined(1:2,:)', R_back);
+ title('back plane');
+ ax(2)=gca;
+ hold on;
+
+ linkaxes(ax,'xy');
+end
+
+%% fit pinhole model
+fprintf('Fitting pinhole model to refined markers ... \n');
+fitopts = struct('square', b_square, 'skew', b_skew, 'distortion', b_distortion);
+[pA,im_res_A] = pinhole_fit_3d_reg(im_mark_refined{1}, re_mark_refined{1}, fitopts);
+[pB,im_res_B] = pinhole_fit_3d_reg(im_mark_refined{2}, re_mark_refined{2}, fitopts);
+im_res_A_rms = rms(im_res_A,2);
+im_res_B_rms = rms(im_res_B,2);
+
+im_proj_A = pinhole_model(re_mark_refined{1}, pA, true);
+im_proj_B = pinhole_model(re_mark_refined{2}, pB, true);
+
+fprintf('Residual:\n');
+fprintf(' C1: %0.3f %0.3f px\n', im_res_A_rms);
+fprintf(' C2: %0.3f %0.3f px\n', im_res_B_rms);
+
+%% measure angle
+% angle is measured in x-z plane, relative to [0;0;1]
+theta1 = atand(-pA.xc(1) / pA.xc(3));
+theta2 = atand(+pB.xc(1) / pB.xc(3));
+fprintf('theta_1 = %6.2f deg\n', theta1);
+fprintf('theta_2 = %6.2f deg\n', theta2);
+
+%% plot
+figure(10);
+clf;
+plot3([pA.xc(1) 0], [pA.xc(2) 0], [pA.xc(3) 0], 'ro-');
+hold on;
+plot3([pB.xc(1) 0], [pB.xc(2) 0], [pB.xc(3) 0], 'go-');
+axis equal tight xy;
+view(0,180);
+
+%% save pinhole model
+fprintf('------------------------------------------------------------------\n');
+fprintf('Camera calibration complete\n');
+fprintf('Saving to file ... ');
+
+s_model = struct();
+s_model.pl_to_gl = eye(3);
+s_model.pl_to_gl_mat= repmat(eye(3),[1 1 2]);
+s_model.gl_plate_pos= [0 0 -0.5; 0 0 +0.5]';
+s_model.gl_origin = [0 0 0]';
+s_model.n_cameras = 2;
+s_model.n_views = 2;
+
+% camera calibration
+pA.im = imA;
+pB.im = imB;
+cA = struct('calib', pA, 'id', 1, 'i_axis', i_axis, 'j_axis', j_axis);
+cB = struct('calib', pB, 'id', 2, 'i_axis', i_axis, 'j_axis', j_axis);
+s_model.camera = [cA, cB];
+
+% bounding volume
+[~, r_sphere] = volume_bounds(s_model);
+r_sphere_lim = min(r_sphere);
+s_model.r_sphere = r_sphere;
+s_model.r_sphere_lim= r_sphere_lim;
+
+save([calib_dir model_file], '-struct', 's_model');
+
+fprintf('done\n');
+
diff --git a/stereo/spiv_imcovar.m b/stereo/spiv_imcovar.m
new file mode 100644
index 0000000000000000000000000000000000000000..7fd69b5db6d4584e352e7c6a3e4472f77e337202
--- /dev/null
+++ b/stereo/spiv_imcovar.m
@@ -0,0 +1,196 @@
+%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
+% measurement of covariance between stereo PIV images, for determination of
+% wall position
+%
+% Author: John Lawson
+% j.m.lawson@soton.ac.uk
+% Created: 29/09/2019
+%
+addpath(genpath([pwd '/../']));
+
+%% input parameters
+
+% path of images for self calibration
+%src_dir = 'H:\davis\multiscale\Gen1_12\U10\U10_x-axis=241.317_x-axis=241.317_x-axis=213.316';
+src_pat = '%s/B%05d.im7';
+src_range = 1 : 4 : 1000;
+n_src = length(src_range);
+
+% path of MATLAB calibration file
+%cal_dir = src_dir;
+%cal_pat = '%s/pinhole.shifted.mat';
+% destination
+dst_dir = src_dir;
+dst_pat = '%s/dewarped.mat';
+
+b_plot_covar = false;
+b_plot_dewarped = false;
+b_plot_source = false;
+
+%% derived parameters
+src_fun = @(n) sprintf(src_pat, src_dir, n);
+cal_file = sprintf(cal_pat, cal_dir);
+
+%% load camera model
+fprintf('loading camera model ...\n');
+s_model = importdata(cal_file);
+i_axis = s_model.camera(1).i_axis;
+j_axis = s_model.camera(1).j_axis;
+c_A = s_model.camera(1).calib;
+c_B = s_model.camera(2).calib;
+
+%% calculate axes for reconstruction
+% based on first-guess in model structure
+pl_to_gl = s_model.pl_to_gl;
+x_0 = s_model.gl_origin;
+P_0 = [pl_to_gl, x_0];
+e_n0 = pl_to_gl(:,3);
+z0 = dot(x_0, e_n0);
+M = [e_n0; -z0]';
+
+%% build a stereo PIV reconstruction object
+fprintf('Building reconstruction object...\n');
+stereo = StereoPlane(s_model.camera, P_0);
+[u_axis,v_axis] = stereo.make_axes([1 1]); % 1 pixel resolution
+stereo.axes(u_axis,v_axis,false);
+n_u = stereo.n_u;
+n_v = stereo.n_v;
+map = stereo.map;
+map(1).jac = [];
+map(2).jac = [];
+map(1).e_c = [];
+map(2).e_c = [];
+
+fprintf('Resolution: %5.1f x %5.1f px/mm\n', stereo.scale);
+fprintf('ROI size: %5.1f x %5.1f mm\n', stereo.W, stereo.H);
+fprintf(' %5d x %5d px\n', n_u, n_v);
+
+%% set up grid for sum-of-correlation
+pl_sz = [n_u n_v];
+I_AB_mean = zeros(pl_sz, 'single');
+I_AA_mean = zeros(pl_sz, 'single');
+I_BB_mean = zeros(pl_sz, 'single');
+I_A_mean = zeros(pl_sz, 'single');
+I_B_mean = zeros(pl_sz, 'single');
+
+%% iterate
+for n = 1 : n_src
+ %% load image
+ src_name = src_fun(src_range(n));
+ fprintf('[%3d of %3d] %s\n', n, n_src, src_name);
+ imx = readimx(src_name);
+ im_A = single(imx.Frames{1}.Components{1}.Planes{1});
+ im_B = single(imx.Frames{3}.Components{1}.Planes{1});
+
+ %% dewarp image
+ %fprintf(' dewarping ...\n');
+ pl_im_A = interpn(i_axis, j_axis, im_A, map(1).im, map(1).jm, 'linear', 0);
+ pl_im_B = interpn(i_axis, j_axis, im_B, map(2).im, map(2).jm, 'linear', 0);
+
+ %% filter
+ %G = fspecial('disk', 3);
+ %pl_im_A = conv2(pl_im_A, G, 'same');
+ %pl_im_B = conv2(pl_im_B, G, 'same');
+
+ %% cross-correlate
+ %fprintf(' cross correlating ...\n');
+ % add to mean
+ I_AA_mean = I_AA_mean + pl_im_A.*pl_im_A / n_src;
+ I_BB_mean = I_BB_mean + pl_im_B.*pl_im_B / n_src;
+ I_AB_mean = I_AB_mean + pl_im_A.*pl_im_B / n_src;
+ I_A_mean = I_A_mean + pl_im_A / n_src;
+ I_B_mean = I_B_mean + pl_im_B / n_src;
+end
+
+%% covariance
+A_var = max(0, I_AA_mean - I_A_mean.^2);
+B_var = max(0, I_BB_mean - I_B_mean.^2);
+AB_covar = I_AB_mean - I_A_mean .* I_B_mean;
+R_AB = AB_covar ./ sqrt(A_var .* B_var);
+
+%% save to file
+dst_name = sprintf(dst_pat, dst_dir);
+fs = struct();
+fs.u = stereo.u;
+fs.v = stereo.v;
+fs.P = stereo.P;
+fs.M = stereo.M;
+fs.cmodel = stereo.cmodel;
+fs.im_mean = {I_A_mean, I_B_mean};
+fs.im_var = {A_var, B_var};
+fs.im_covar = AB_covar;
+fs.src_range = src_range;
+fs.src_pat = src_pat;
+fs.src_dir = src_dir;
+fprintf('saving to %s ...\n', dst_name);
+save(dst_name, '-struct', 'fs');
+fprintf('done\n');
+
+%% plot correlation
+if b_plot_covar
+ figure(1);
+ clf;
+ subplot(3,1,1);
+ imagesc(stereo.u, stereo.v, R_AB');
+ axis equal tight xy
+ title('Image covariance');
+ clim([0 0.5]);
+ ylim([-72 -60]);
+
+ subplot(3,1,2);
+ imagesc(stereo.u, stereo.v, I_A_mean');
+ axis equal tight xy
+ title('A mean');
+ clim([0 512]);
+ ylim([-72 -60]);
+
+ subplot(3,1,3);
+ imagesc(stereo.u, stereo.v, I_B_mean');
+ axis equal tight xy
+ title('B mean');
+ clim([0 512]);
+ ylim([-72 -60]);
+ %set(gca,'clim', [0 0.08]);
+end
+
+%% display dewarped images
+if b_plot_dewarped
+ figure(3);
+ clf;
+ subplot(1,2,1);
+ ax = gca;
+ imagesc(stereo.u, stereo.v, pl_im_A');
+ axis equal xy;
+ xlim(stereo.u([1 end]));
+ ylim(stereo.v([1 end]));
+ title('Camera 1, dewarped');
+ %ylim(j_axis([end-512 end]));
+ %xlim(i_axis([1 end]));
+
+ subplot(1,2,2);
+ ax(2)=gca;
+ imagesc(stereo.u, stereo.v, pl_im_B');
+ axis equal xy;
+ xlim(stereo.u([1 end]));
+ ylim(stereo.v([1 end]));
+ title('Camera 2, dewarped');
+
+ %quiver(um, vm,-squeeze(uv_shift(1,:,:)),-squeeze(uv_shift(2,:,:)), 'r-');
+
+ linkaxes(ax, 'xy');
+end
+
+%% plot raw images
+if b_plot_source
+ figure(4);
+ clf;
+ subplot(1,2,1);
+ imagesc(i_axis, j_axis, im_A');
+ axis equal tight ij;
+ title('Camera 2, raw');
+
+ subplot(1,2,2);
+ imagesc(i_axis, j_axis, im_B');
+ axis equal tight ij;
+ title('Camera 1, raw');
+end
\ No newline at end of file
diff --git a/stereo/spiv_mask.m b/stereo/spiv_mask.m
new file mode 100644
index 0000000000000000000000000000000000000000..023beb6d9ba285be9797b812867acf04118df64f
--- /dev/null
+++ b/stereo/spiv_mask.m
@@ -0,0 +1,119 @@
+%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
+% algorithmically generate a mask for PIV processing
+%
+% Author: John Lawson
+% j.m.lawson@soton.ac.uk
+% Created: 27/06/2019
+%
+addpath(genpath([pwd '/../../piv2d/']));
+addpath([pwd '/../../misc/mtimesx/']);
+
+%% input parameters
+% path of images for self calibration
+src_dir = 'H:\davis\multiscale\Gen1\U10\U10_x-axis=107.986_x-axis=107.985_x-axis=79.9874\';
+src_pat = '%s/B%05d.im7';
+src_range = 1 : 100 : 1000;
+src_fun = @(n) sprintf(src_pat, src_dir, n);
+n_src = length(src_range);
+wsize = [1 11];
+
+%% load prototype image
+src_name = src_fun(src_range(1));
+fprintf('load %s ...\n', src_name);
+imx = readimx(src_name);
+n_frames = length(imx.Frames);
+im_size = size(imx.Frames{1}.Components{1}.Planes{1});
+
+im_min_ary = arrayfun(@(x) zeros(im_size), 1:n_frames, 'UniformOutput', false);
+im_max_ary = arrayfun(@(x) zeros(im_size), 1:n_frames, 'UniformOutput', false);
+im_mean_ary = arrayfun(@(x) zeros(im_size), 1:n_frames, 'UniformOutput', false);
+im_var_ary = arrayfun(@(x) zeros(im_size), 1:n_frames, 'UniformOutput', false);
+
+%% gather image statistics
+fprintf('------------------------------------------------------------------\n');
+fprintf('PROCESSING START %s\n', datestr(now));
+G = fspecial('log', wsize, 0.1*wsize(2));
+
+
+for n = 1 : n_src
+ %% load image
+ src_name = src_fun(src_range(n));
+ fprintf('[%3d of %3d] %s\n', n, n_src, src_name);
+ imx = readimx(src_name);
+ src_ary = cellfun(@(f) double(f.Components{1}.Planes{1}), imx.Frames, 'UniformOutput', false);
+
+ %% time filter to accentuate particle images
+ % particle images tend to change over frames, background does not
+ im_ary(1:2) = {src_ary{2} - src_ary{1}};
+ im_ary(3:4) = {src_ary{4} - src_ary{3}};
+
+ %% add to average
+ for f = 1 : n_frames
+ % high pass filter to emphasise particle images
+ im_filt = conv2(im_ary{f}, G, 'same');
+
+ %im_filt = double(maxfiltnd(single(im_ary{f}), wsize));
+ %im_filt = conv2(im_ary{f}, G, 'same');
+
+ im_min_ary{f} = min(im_min_ary{f}, im_filt);
+ im_max_ary{f} = max(im_max_ary{f}, im_filt);
+ im_mean_ary{f} = im_mean_ary{f} + im_filt / n_src;
+ im_var_ary{f} = im_var_ary{f} + im_filt.^2 / n_src;
+ end
+end
+
+for f = 1 : n_frames
+ im_var_ary{f} = max(0,im_var_ary{f} - im_mean_ary{f}.^2);
+ im_std_ary{f} = sqrt(im_var_ary{f});
+end
+
+%% display
+figure(1);
+clf;
+s_el = strel('disk', 5);
+ir = 1 : im_size(1);
+jr = 1 : im_size(2) - 500;
+cmap = jet;
+cmap(1,:) = 0;
+j_axis = 1 : im_size(2);
+
+for f = 1 : n_frames
+ %% compute threshold
+ im_std = sqrt(im_var_ary{f});
+ im_samp = im_std(ir,jr);
+ thresh = prctile(im_samp(:), 2);
+
+ %% mask
+ mask = im_var_ary{f} > thresh^2;
+ mask = imerode(mask, s_el);
+ mask = imdilate(mask, s_el);
+ %{
+ [label,n_labels] = bwlabel(mask);
+
+ n_l = zeros(1,n_labels);
+ for k = 1 : n_labels
+ n_l(k)= nnz(label == k);
+ end
+ [~,l_max]= max(n_l);
+ mask = label == l_max;
+ mask = imfill(mask, 'holes');
+
+ %% find boundary
+ im = 1 : im_size(1);
+ jm = zeros(1, im_size(1));
+ for ii = im
+ j_min = find(~mask(ii,:) & j_axis > jr(end), 1, 'first');
+ if isempty(j_min)
+ j_min = jr(end);
+ end
+ jm(ii)= j_min;
+ end
+ %}
+ %% plot
+ subplot(1,n_frames,f);
+ imagesc(mask');
+ axis equal tight ij;
+ hold on;
+ %plot(im,jm,'r-');
+ %colormap(cmap);
+end
diff --git a/stereo/spiv_mask_manual.m b/stereo/spiv_mask_manual.m
new file mode 100644
index 0000000000000000000000000000000000000000..207c64b7e2d42028dcfe64417e0c675d13c9d935
--- /dev/null
+++ b/stereo/spiv_mask_manual.m
@@ -0,0 +1,177 @@
+%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
+% algorithmically generate a mask for PIV processing
+%
+% Author: John Lawson
+% j.m.lawson@soton.ac.uk
+% Created: 27/06/2019
+%
+close all; clear; clc;
+addpath([pwd '/../readimx']);
+
+%% input parameters
+% path of images for self calibration
+src_root = 'H:\davis\multiscale\Gen1_12\U10\';
+% pattern to match source directories
+src_dir_pat = '^U10_x-axis=';
+
+% usually, you will not need to change the following parameters:
+src_pat = '%s/%s/B00001.im7'; % pattern for source image input
+msk_pat = '%s/%s/MASK/B0001.im7'; % pattern for mask image output
+mat_pat = '%s/%s/mask.mat'; % patlab for MATLAB .mat output
+y_lim = 4872 + [-750 0]; % ROI
+c_lim = [-1 1]*128; %
+font_size = 24; % figure font size
+
+%% list all directories
+src_dir_list = dir(src_root);
+b_match = arrayfun(@(f) ~isempty(regexp(f.name, src_dir_pat, 'once')) && f.isdir, src_dir_list);
+src_dir_list = src_dir_list(b_match);
+n_src = length(src_dir_list);
+
+fprintf('In root %s\n', src_root);
+fprintf(' %s\n', src_dir_list.name);
+fprintf(' %d directories to process\n', n_src);
+
+%% draw figure
+cmap = jet;
+cmap(1,:) = 0;
+
+%% loop over files to be processed
+for n = 1 : n_src
+ %% check for existing files
+ src_name = sprintf(src_pat, src_root, src_dir_list(n).name);
+ msk_name = sprintf(msk_pat, src_root, src_dir_list(n).name);
+ mat_name = sprintf(mat_pat, src_root, src_dir_list(n).name);
+ fprintf('load %s \n', src_name);
+ if ~exist(src_name, 'file')
+ warning('source %s does not exist, skipping\n', src_name);
+ continue;
+ end
+
+ if exist(msk_name, 'file')
+ s = input('mask already exists, do you wish to overwrite? [y/N] ', 's');
+ b_overwrite = strcmpi(s, 'y');
+ if ~b_overwrite
+ fprintf('OK, skipping ...\n');
+ continue;
+ end
+ end
+
+ %% make mask directory if necessary
+ msk_dir = fileparts(msk_name);
+ if ~exist(msk_dir, 'dir')
+ mkdir(msk_dir);
+ end
+
+ %% load prototype image
+ fprintf(' loading ... \n');
+ imx = readimx(src_name);
+ n_frames = length(imx.Frames);
+ im_size = size(imx.Frames{1}.Components{1}.Planes{1});
+ src_ary = cellfun(@(f) double(f.Components{1}.Planes{1}), imx.Frames, 'UniformOutput', false);
+
+ im_ary = {src_ary{1}-src_ary{2}, src_ary{3}-src_ary{4}};
+ n_mask = length(im_ary);
+ msk_ary = im_ary;
+ msk_corners = cell(1,n_mask);
+
+ %% loop over number of masks
+ for m = 1 : n_mask
+ %% loop until we get an acceptable mask
+ b_continue = true;
+ msk_ary{m} = false(im_size);
+ while b_continue
+ %% render image
+ figure(1);
+ clf;
+
+ subplot(2,1,1);
+ imagesc((src_ary{(m-1)*2+1})');
+ axis equal tight ij;
+ ylim(y_lim);
+ set(gca, 'clim', [0 512]);
+ colormap(gca, parula);
+ title('Source image');
+
+ drawnow;
+
+
+ subplot(2,1,2);
+ imagesc((im_ary{m})');
+ axis equal tight ij;
+ ylim(y_lim);
+ colormap(gca, redblue);
+ set(gca, 'clim', c_lim);
+ drawnow;
+
+ %% specify masked region
+ fprintf('Select region to mask out\n');
+ fprintf(' right click to add points\n');
+ fprintf(' left click to finish adding points\n');
+ fprintf(' context menu -> create mask to finalise mask\n');
+ title('Select region to mask out', 'fontsize', font_size);
+ [mask, ic, jc] = roipoly;
+ mask = mask';
+
+ %% present masked region
+ new_mask = msk_ary{m} | mask;
+ imagesc(((~new_mask).*im_ary{m})');
+ axis equal tight ij;
+ ylim(y_lim);
+ drawnow;
+ set(gca, 'clim', c_lim);
+ set(gca,'colormap', redblue);
+ title('Finish [f] / Repeat [r] / Add another [a]?', 'fontsize', font_size);
+ s = input('Finish [f] / Repeat (try again) [r] / Add another region [a]? [F] ', 's');
+
+ if strcmpi(s, 'R')
+ %% repeat
+ continue;
+ elseif strcmpi(s, 'A')
+ %% add current region, but continue
+ msk_ary{m} = new_mask;
+ msk_corners{m}{end+1} = [ic,jc];
+ else
+ %% add current region and finish
+ msk_ary{m} = new_mask;
+ msk_corners{m}{end+1} = [ic,jc];
+ b_continue = false;
+ end
+ end
+ end
+
+ %% add mask field to IMX
+ msk_map = [1 1 2 2];
+ for f = 1 : 4
+ imx.Frames{f}.ComponentNames{2} = 'MASK';
+ imx.Frames{f}.Components{2} = imx.Frames{f}.Components{1};
+ imx.Frames{f}.Components{2}.Name = 'MASK';
+ imx.Frames{f}.Components{2}.Planes{1} = ~msk_ary{msk_map(f)};
+ end
+
+ %% save mask to file
+ fprintf('save mask to %s\n', msk_name);
+ if exist(msk_name, 'file') && ~b_overwrite
+ warning('Mask file already exists. Cannot overwrite.');
+ continue;
+ end
+ writeimx(imx, msk_name);
+
+ %% copy .set
+ src_set = sprintf('%s/%s.set', src_root, src_dir_list(n).name);
+ msk_set = sprintf('%s/%s/Mask.set', src_root, src_dir_list(n).name);
+ if ~exist(src_set, 'file')
+ warning('Source .set file for mask not present');
+ elseif exist(msk_set, 'file')
+ warning('Mask .set file already present. skipping');
+ else
+ fprintf('copy %s -> %s\n', src_set, msk_set);
+ copyfile(src_set, msk_set);
+ end
+
+ %% save matlab version
+ fs = struct();
+ fs.mask = msk_ary;
+ fs.corners = msk_corners;
+ save(mat_name, '-struct', 'fs');
+end
\ No newline at end of file
diff --git a/stereo/spiv_reconstruct.m b/stereo/spiv_reconstruct.m
new file mode 100644
index 0000000000000000000000000000000000000000..00f2d104a94760305014040e5d0bfffee107652b
--- /dev/null
+++ b/stereo/spiv_reconstruct.m
@@ -0,0 +1,218 @@
+%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
+% perform reconstruction of stereo PIV vector fields by combining separate
+% planar PIV vector fields
+%
+% Author: John Lawson
+% j.m.lawson@soton.ac.uk
+% Created: 26/06/2019
+%
+%
+addpath(genpath([fileparts(mfilename('fullpath')) '/../']));
+
+%% input parameters
+% source of PIV data
+%piv_dt = 35E-6; % hard code PIV delta t for now
+%src_dir = 'M:\multiscale\Gen1\U10_x-axis=382.988_x-axis=382.985_x-axis=354.987\C12\';
+src_pat = '%s/B%05d.mat';
+src_range = 1:1000;
+src_fun = @(n) sprintf(src_pat, src_dir, n);
+% path of camera model
+%calib_dir = [src_dir '/../'];
+%calib_file = 'pinhole.shifted.mat';
+% destination for processed files
+dst_dir = src_dir;
+dst_pat = '%s/V%05d.mat';
+dst_fun = @(n) sprintf(dst_pat, dst_dir, n);
+
+b_plot_recon = false;
+b_plot_error = false;
+
+b_overwrite = true;
+
+%% emit warning
+if b_overwrite
+ warning('files will be overwritten if they already exist');
+end
+
+%% load calibration
+f_model = [calib_dir calib_file];
+fprintf('Load calibration %s ...\n', f_model);
+s_model = load(f_model);
+
+%% load a sample vector field
+src_name = src_fun(src_range(1));
+fprintf('Load %s\n', src_name);
+[VC1, VC2] = loadvec_pair(src_name);
+
+if exist('piv_dt', 'var')
+ warning('Override delta t, dt = %f us', piv_dt*1E6);
+else
+ piv_dt = VC1.dt;
+end
+
+
+% modify axis limits
+s_model.camera(1).i_axis = VC1.X(:,1)';
+s_model.camera(1).j_axis = VC1.Y(1,:);
+s_model.camera(2).i_axis = VC2.X(:,1)';
+s_model.camera(2).j_axis = VC2.Y(1,:);
+
+%% build stereo reconstruction model
+fprintf(' building calibration model...\n');
+P = [s_model.pl_to_gl, s_model.gl_origin];
+stereo = StereoPlane(s_model.camera, P);
+% define grid for stereo vector field interpolation
+% average vector spacing
+di = s_model.camera(1).i_axis([1 2]) * [-1;1];
+dj = s_model.camera(1).j_axis([1 2]) * [-1;1];
+[u_axis,v_axis] = stereo.make_axes([di dj]);
+stereo.axes(u_axis, v_axis);
+% rotation
+pl_to_gl = stereo.P(:,1:3);
+n_u = stereo.n_u;
+n_v = stereo.n_v;
+
+%% loop to process fields
+fprintf('------------------------------------------------------------------\n');
+fprintf('PROCESSING START %s\n', datestr(now));
+
+n_src = length(src_range);
+
+for n = 1 : n_src
+ %% load planar vector fields
+ src_name = src_fun(src_range(n));
+ dst_name = dst_fun(src_range(n));
+ fprintf('[%3d of %3d] %s\n', n, n_src, src_name);
+ if ~exist(src_name, 'file')
+ warning('source does not exist, skipping');
+ continue;
+ end
+ if exist(dst_name, 'file') && ~b_overwrite
+ warning('destination already exists, skipping');
+ continue;
+ end
+
+ [VC1,VC2] = loadvec_pair(src_name);
+
+ % account for PIV delta-t being different in source fields
+ % this can occur if we require output in pixel units
+ VC1 = rescale_field(VC1, VC1.dt/piv_dt);
+ VC2 = rescale_field(VC2, VC2.dt/piv_dt);
+
+ %% extract mask
+ M1 = (VC1.U==0 & VC1.V == 0) | isnan(VC1.U) | isnan(VC1.V);
+ M2 = (VC2.U==0 & VC2.V == 0) | isnan(VC2.U) | isnan(VC2.V);
+ VC1.U(M1) = nan;
+ VC1.V(M1) = nan;
+ VC2.U(M2) = nan;
+ VC2.V(M2) = nan;
+
+ %% reconstruct velocity
+ % get coordinates of vectors in world system
+ n_vec = n_u * n_v;
+ gl_pos = stereo.P * [stereo.um(:) stereo.vm(:) zeros(n_vec,1) ones(n_vec,1)]';
+ gl_pos = reshape(gl_pos.', [n_u n_v 3]);
+ % reconstruct velocity in world system
+ [~,gl_vel,v1,v2]= stereo.reconstruct(VC1, VC2);
+ gl_vel = gl_vel / 1E3;
+ % direction of common measurement vector
+ e_c = reshape(stereo.map(1).e_c, [3 1 n_u n_v]);
+ e_c = mtimesx(pl_to_gl, e_c);
+ b_mask = isnan(gl_vel(:,:,1));
+
+ %% store in structure
+ V = struct( 'X', gl_pos(:,:,1), ...
+ 'Y', gl_pos(:,:,2), ...
+ 'Z', gl_pos(:,:,3), ...
+ 'U', gl_vel(:,:,1), ...
+ 'V', gl_vel(:,:,2), ...
+ 'W', gl_vel(:,:,3), ...
+ 'mask', b_mask, ...
+ 'v1', v1, ...
+ 'v2', v2, ...
+ 'e_v', e_c, ...
+ 'u_axis', stereo.u, ...
+ 'v_axis', stereo.v, ...
+ 'P', stereo.P, ...
+ 'dt', piv_dt);
+ %% check for crazy answers
+ if nnz(abs(V.U) > 20) > 0.01*numel(V.U)
+ warning('it''s happened agaiiiiin...');
+ end
+
+ %% save to file
+ %fprintf(' save %s\n', dst_name);
+ if exist(dst_name, 'file') && ~b_overwrite
+ warning('file already exists, will not overwrite');
+ else
+ parsave(dst_name, V);%, '-v7.3', '-nocompression');
+ end
+
+ %% plot reconstructed field
+ if b_plot_recon
+ figure(1);
+ clf;
+ surf(V.X,V.Y,V.Z,V.U,'EdgeColor','none');
+ hold on;
+ rx=1:4:n_u;
+ ry=1:4:n_v;
+ quiver3sc(V.X(rx,ry), V.Y(rx,ry), V.Z(rx,ry), V.U(rx,ry), V.V(rx,ry), V.W(rx,ry), 0.5, 'k');
+ set(gca,'clim',[0 10]);
+ axis equal tight xy;
+ view(0,90);
+ end
+
+ %% plot residual error in co-measured component
+ if b_plot_error
+ figure(2);
+ clf;
+ subplot(1,3,1);
+ imagesc(stereo.u, stereo.v, v1' / 1E3);
+ axis equal tight xy;
+ hold on;
+ ax = gca;
+ title('Camera 1, v component');
+ %quiver(F1.X, F1.Y, F1.U, F1.V, 'k');
+ set(gca,'clim',[-2 2]);
+
+ subplot(1,3,2);
+ imagesc(stereo.u, stereo.v, v2'/ 1E3);
+ axis equal tight xy;
+ hold on;
+ ax(2) = gca;
+ title('Camera 2, v component');
+ %quiver(F2.X, F2.Y, F2.U, F2.V, 'k');
+ set(gca,'clim',[-2 2]);
+
+ subplot(1,3,3);
+ imagesc(stereo.u, stereo.v, (v1-v2)'/1E3);
+ axis equal tight xy;
+ title('Difference, v component');
+ set(gca,'clim',[-1 1]*2);
+ set(gca,'colormap',redblue);
+ ax(3) = gca;
+
+
+ linkaxes(ax, 'xy');
+ end
+
+ %% measure residual error
+ continue;
+ dv = 1/2*(v1-v2);
+ sv = 1/2*(v1+v2);
+ dx = mean(diff(stereo.u));
+ dy = mean(diff(stereo.v));
+ [dvdx,dvdy] = lsqgradient2d(sv, dx, dy);
+ d2vx = conv2(sv, [-1 2 -1]' / dx^2, 'same');
+ d2vy = conv2(sv, [-1 2 -1] / dx^2, 'same');
+
+ dvdx = conv2(dvdx, ones(10), 'same');
+ ok = ~isnan(sv + dvdx + dvdy);
+ ok(:,1:100)=false; % manually mask shitty region
+ disp(std(sv(ok)));
+ disp(std(dv(ok)));
+ disp(corrcoef(dvdx(ok), dv(ok)));
+end
+
+fprintf('------------------------------------------------------------------\n');
+fprintf('PROCESSING COMPLETE %s\n', datestr(now));
\ No newline at end of file
diff --git a/stereo/spiv_reconstruct_davis.m b/stereo/spiv_reconstruct_davis.m
new file mode 100644
index 0000000000000000000000000000000000000000..000653bc5a6831a901211cfe355f0f2725b623bf
--- /dev/null
+++ b/stereo/spiv_reconstruct_davis.m
@@ -0,0 +1,184 @@
+%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
+% perform reconstruction of stereo PIV vector fields by combining separate
+% planar PIV vector fields
+%
+% Author: John Lawson
+% j.m.lawson@soton.ac.uk
+% Created: 26/06/2019
+%
+addpath(genpath([pwd '/../']));
+
+%% input parameters
+% source of PIV data
+%src_dir = 'H:\davis\multiscale\Gen1\U10\U10_x-axis=82.986_x-axis=82.9855_x-axis=54.9874\SeqPIV_MP(2x32x32_75%ov)_GPU_C12\';
+%src_dir = 'H:\davis\multiscale\Gen1_12\U10\U10_x-axis=266.32_x-axis=266.317_x-axis=238.315/PIV_MP(2x32x32_75%ov)_GPU_C12/';
+src_pat = '%s/B%05d.vc7';
+src_range = 1 : 11;
+src_fun = @(n) sprintf(src_pat, src_dir, n);
+% path of camera model
+calib_dir = [src_dir '/../'];
+calib_file = 'pinhole.shifted.mat';
+% destination for processed files
+dst_dir = src_dir;
+dst_pat = '%s/B%05d.mat';
+dst_fun = @(n) sprintf(dst_pat, dst_dir, n);
+
+b_plot_recon = false;
+b_plot_error = false;
+
+%% load calibration
+f_model = [calib_dir calib_file];
+fprintf('Load calibration %s ...\n', f_model);
+s_model = importdata(f_model);
+
+%% load a sample vector field
+src_name = src_fun(src_range(1));
+fprintf('Load %s\n', src_name);
+F = readimx(src_name);
+VC1 = create2DVec(F.Frames{1});
+VC2 = create2DVec(F.Frames{2});
+% get PIV delta t
+attrs = cell2mat(F.Attributes);
+i = find(strcmp({attrs.Value}, 'Reference time dt 1'));
+name = attrs(i).Name;
+name = strrep(name, 'DevDataName', 'DevDataTrace');
+name = strrep(name, 'DevDataAlias', 'DevDataTrace');
+warning('Override delta t');
+piv_dt = 40E-6; double(F.Attributes{strcmp({attrs.Name}, name)}.Value) * 1E-6;
+
+% modify axis limits
+s_model.camera(1).i_axis = VC1.X(:,1)';
+s_model.camera(1).j_axis = VC1.Y(1,:);
+s_model.camera(2).i_axis = VC2.X(:,1)';
+s_model.camera(2).j_axis = VC2.Y(1,:);
+
+%% build stereo reconstruction model
+fprintf(' building calibration model...\n');
+P = [s_model.pl_to_gl, s_model.gl_origin];
+stereo = StereoPlane(s_model.camera, P);
+% define grid for stereo vector field interpolation
+% average vector spacing
+di = s_model.camera(1).i_axis([1 2]) * [-1;1];
+dj = s_model.camera(1).j_axis([1 2]) * [-1;1];
+[u_axis,v_axis] = stereo.make_axes([di dj]);
+stereo.axes(u_axis, v_axis);
+% rotation
+pl_to_gl = stereo.P(:,1:3);
+n_u = stereo.n_u;
+n_v = stereo.n_v;
+
+%% loop to process fields
+fprintf('------------------------------------------------------------------\n');
+fprintf('PROCESSING START %s\n', datestr(now));
+
+n_src = length(src_range);
+
+for n = 1 : n_src
+ %% load planar vector fields
+ src_name = src_fun(src_range(n));
+ fprintf('[%3d of %3d]:\n', n, n_src);
+ fprintf(' %s\n', src_name);
+ F = readimx(src_name);
+ VC1 = create2DVec(F.Frames{1});
+ VC2 = create2DVec(F.Frames{2});
+
+ % account for PIV delta -t
+ VC1 = rescale_field(VC1, 1/piv_dt);
+ VC2 = rescale_field(VC2, 1/piv_dt);
+
+ %% reconstruct velocity
+ % get coordinates of vectors in world system
+
+ n_vec = n_u * n_v;
+ gl_pos = stereo.P * [stereo.um(:) stereo.vm(:) zeros(n_vec,1) ones(n_vec,1)]';
+ gl_pos = reshape(gl_pos.', [n_u n_v 3]);
+ % reconstruct velocity in world system
+ [~,gl_vel,v1,v2]= stereo.reconstruct(VC1, VC2);
+ gl_vel = gl_vel / 1E3;
+ % direction of common measurement vector
+ e_c = reshape(stereo.map(1).e_c, [3 1 n_u n_v]);
+ e_c = mtimesx(pl_to_gl, e_c);
+
+ %% store in structure
+ V = struct( 'X', gl_pos(:,:,1), ...
+ 'Y', gl_pos(:,:,2), ...
+ 'Z', gl_pos(:,:,3), ...
+ 'U', gl_vel(:,:,1), ...
+ 'V', gl_vel(:,:,2), ...
+ 'W', gl_vel(:,:,3), ...
+ 'v1', v1, ...
+ 'v2', v2, ...
+ 'e_v', e_c, ...
+ 'P', stereo.P, ...
+ 'dt', piv_dt);
+ V.Attributes = F.Attributes;
+
+ %% save to file
+ dst_name = dst_fun(src_range(n));
+ fprintf(' save %s\n', dst_name);
+ save(dst_name, '-struct', 'V');
+
+ %% plot reconstructed field
+ if b_plot_recon
+ figure(1);
+ clf;
+ surf(V.X,V.Y,V.Z,V.U,'EdgeColor','none');
+ hold on;
+ rx=1:4:n_u;
+ ry=1:4:n_v;
+ quiver3sc(V.X(rx,ry), V.Y(rx,ry), V.Z(rx,ry), V.U(rx,ry), V.V(rx,ry), V.W(rx,ry), 0.5, 'k');
+ set(gca,'clim',[0 10]);
+ axis equal tight xy;
+ view(0,90);
+ end
+
+ %% plot residual error in co-measured component
+ if b_plot_error
+ figure(2);
+ clf;
+ subplot(1,3,1);
+ imagesc(stereo.u, stereo.v, v1' / 1E3);
+ axis equal tight xy;
+ hold on;
+ ax = gca;
+ title('Camera 1, v component');
+ %quiver(F1.X, F1.Y, F1.U, F1.V, 'k');
+ set(gca,'clim',[-2 2]);
+
+ subplot(1,3,2);
+ imagesc(stereo.u, stereo.v, v2'/ 1E3);
+ axis equal tight xy;
+ hold on;
+ ax(2) = gca;
+ title('Camera 2, v component');
+ %quiver(F2.X, F2.Y, F2.U, F2.V, 'k');
+ set(gca,'clim',[-2 2]);
+
+ subplot(1,3,3);
+ imagesc(stereo.u, stereo.v, (v1-v2)'/1E3);
+ axis equal tight xy;
+ title('Difference, v component');
+ set(gca,'clim',[-1 1]*2);
+ set(gca,'colormap',redblue);
+ ax(3) = gca;
+
+
+ linkaxes(ax, 'xy');
+ end
+
+ %% measure residual error
+ dv = 1/2*(v1-v2);
+ sv = 1/2*(v1+v2);
+ dx = mean(diff(stereo.u));
+ dy = mean(diff(stereo.v));
+ [dvdx,dvdy] = lsqgradient2d(sv, dx, dy);
+ d2vx = conv2(sv, [-1 2 -1]' / dx^2, 'same');
+ d2vy = conv2(sv, [-1 2 -1] / dx^2, 'same');
+
+ dvdx = conv2(dvdx, ones(10), 'same');
+ ok = ~isnan(sv + dvdx + dvdy);
+ ok(:,1:100)=false; % manually mask shitty region
+ disp(std(sv(ok)));
+ disp(std(dv(ok)));
+ disp(corrcoef(dvdx(ok), dv(ok)));
+end
diff --git a/stereo/spiv_selfcal.m b/stereo/spiv_selfcal.m
new file mode 100644
index 0000000000000000000000000000000000000000..6f66adb9b4dae6277fb772c56f195bc5ec2dea59
--- /dev/null
+++ b/stereo/spiv_selfcal.m
@@ -0,0 +1,392 @@
+%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
+% self-calibration for position of laser sheet
+%
+% Author: John Lawson
+% j.m.lawson@soton.ac.uk
+% Created: 26/06/2019
+%
+function spiv_selfcal(opts)
+addpath(genpath([pwd '/../']));
+addpath([pwd '/../../misc']);
+
+%% input parameters
+% user specified input parameters
+% overridden when caller specifies these
+b_interactive = nargin < 1;
+
+% path of images for self calibration
+src_dir = 'H:\davis\multiscale\Gen1_123\U10\U10_x-axis=632.986_x-axis=632.985_x-axis=604.987';
+src_pat = '%s/B%05d.im7';
+src_range = 1 : 50 : 1000;
+n_src = length(src_range);
+
+% path of MATLAB calibration file
+cal_dir = src_dir;
+cal_pat = '%s/plate_900.rotated.mat';
+
+% cross correlation options
+wsize = [256 256]*1;
+
+% if called with arguments, extract these
+if ~b_interactive
+ % extract
+ fn = fieldnames(opts);
+ for n = 1 : length(fn)
+ %assignin('base', fn{n}, opts.(fn{n}));
+ eval(sprintf('%s=opts.(fn{n});', fn{n}));
+ end
+end
+
+%% derived parameters
+src_fun = @(n) sprintf(src_pat, src_dir, n);
+cal_file = sprintf(cal_pat, cal_dir);
+
+%% load camera model
+fprintf('loading camera model ...\n');
+s_model = importdata(cal_file);
+i_axis = s_model.camera(1).i_axis;
+j_axis = s_model.camera(1).j_axis;
+c_A = s_model.camera(1).calib;
+c_B = s_model.camera(2).calib;
+
+%% calculate axes for reconstruction
+% based on first-guess in model structure
+pl_to_gl = s_model.pl_to_gl;
+x_0 = s_model.gl_origin;
+P_0 = [pl_to_gl, x_0];
+e_n0 = pl_to_gl(:,3);
+z0 = dot(x_0, e_n0);
+M = [e_n0; -z0]';
+
+%% build a stereo PIV reconstruction object
+fprintf('Building reconstruction object...\n');
+stereo = StereoPlane(s_model.camera, P_0);
+[u_axis,v_axis] = stereo.make_axes([1 1]); % 1 pixel resolution
+stereo.axes(u_axis,v_axis,false);
+
+fprintf('Resolution: %5.1f x %5.1f px/mm\n', stereo.scale);
+fprintf('ROI size: %5.1f x %5.1f mm\n', stereo.W, stereo.H);
+fprintf(' %5d x %5d px\n', stereo.n_u, stereo.n_v);
+
+%% set up grid for sum-of-correlation
+n_windows = floor([stereo.n_u stereo.n_v]./wsize);
+ii_window = (1:wsize(1)) - wsize(1)/2;
+jj_window = (1:wsize(2)) - wsize(2)/2;
+
+ii0 = floor(mod(stereo.n_u, wsize(1))/2);
+jj0 = floor(mod(stereo.n_v, wsize(2))/2);
+ir = ii0 + (1 : n_windows(1)*wsize(1));
+jr = jj0 + (1 : n_windows(2)*wsize(2));
+ii_ctr = ir(1 : wsize(1) : end) + wsize(1)/2;
+jj_ctr = jr(1 : wsize(2) : end) + wsize(2)/2;
+u_ctr = stereo.u(ii_ctr);
+v_ctr = stereo.v(jj_ctr);
+
+window = @(x) permute(reshape(x, [wsize(1) n_windows(1) wsize(2) n_windows(2)]), [1 3 2 4]);
+mosaic = @(x) reshape(permute(x, [1 3 2 4]), [wsize(1)*n_windows(1) wsize(2)*n_windows(2)]);
+
+C_AB_mean = zeros([wsize n_windows]);
+C_AA_mean = zeros([wsize n_windows]);
+C_BB_mean = zeros([wsize n_windows]);
+I_A_mean = zeros([wsize n_windows]);
+I_B_mean = zeros([wsize n_windows]);
+
+%% iterate
+for n = 1 : n_src
+ %% load image
+ src_name = src_fun(src_range(n));
+ fprintf('[%3d of %3d]\n', n, n_src);
+ fprintf(' load %s ...\n', src_name);
+ imx = readimx(src_name);
+ imA = double(imx.Frames{1}.Components{1}.Planes{1});
+ imB = double(imx.Frames{3}.Components{1}.Planes{1});
+
+ %% dewarp image
+ fprintf(' dewarping ...\n');
+ pl_imA = interpn(i_axis, j_axis, imA, stereo.map(1).im, stereo.map(1).jm, 'linear', 0);
+ pl_imB = interpn(i_axis, j_axis, imB, stereo.map(2).im, stereo.map(2).jm, 'linear', 0);
+
+ %% cross-correlate
+ fprintf(' cross correlating ...\n');
+ imA_window = window(pl_imA(ir,jr));
+ imB_window = window(pl_imB(ir,jr));
+ % fourier transform
+ imA_ft = fft2(imA_window);
+ imB_ft = fft2(imB_window);
+ % subtract mean
+ imA_ft(1,1,:,:)=0;
+ imB_ft(1,1,:,:)=0;
+ % measure energy
+ %E_A = squeeze(sum(sum(abs(imA_ft).^2, 2),1)) / prod(wsize);
+ %E_B = squeeze(sum(sum(abs(imB_ft).^2, 2),1)) / prod(wsize);
+ % correlate
+ C_AB = imA_ft .* conj(imB_ft);
+ C_AA = abs(imA_ft).^2;
+ C_BB = abs(imB_ft).^2;
+ % add to mean
+ C_AB_mean = C_AB_mean + C_AB / n_src;
+ C_AA_mean = C_AA_mean + C_AA / n_src;
+ C_BB_mean = C_BB_mean + C_BB / n_src;
+ %E_A_mean = E_A_mean + E_A / n_src;
+ %E_B_mean = E_B_mean + E_B / n_src;
+ I_A_mean = I_A_mean + imA_ft / n_src;
+ I_B_mean = I_B_mean + imB_ft / n_src;
+end
+
+%% inverse transform to get correlation
+R_AB_mean = ifft2(C_AB_mean - I_A_mean .* conj(I_B_mean), 'symmetric');
+R_AA_mean = ifft2(C_AA_mean - I_A_mean .* conj(I_A_mean), 'symmetric');
+R_BB_mean = ifft2(C_BB_mean - I_B_mean .* conj(I_B_mean), 'symmetric');
+% shift correlation, peak now occurs relative to index wsize/2+1
+R_AB_mean = fftshift(fftshift(R_AB_mean,2),1);
+R_AA_mean = fftshift(fftshift(R_AA_mean,2),1);
+R_BB_mean = fftshift(fftshift(R_BB_mean,2),1);
+% normalise correlation with average energy
+E_A_mean = squeeze(sum(sum(C_AA_mean,1),2)) / prod(wsize);
+E_B_mean = squeeze(sum(sum(C_BB_mean,1),2)) / prod(wsize);
+R_AB_norm = R_AB_mean./sqrt(shiftdim(E_A_mean.*E_B_mean,-2));
+
+%% measure correlation peak
+fprintf('Measuring correlation peak ...\n');
+uv_shift = zeros([2 n_windows]);
+pk_mag = zeros(n_windows);
+sig_fit = zeros([2 n_windows]);
+du = stereo.u(2) - stereo.u(1);
+dv = stereo.v(2) - stereo.v(1);
+duv = diag([du dv]);
+for kk = 1 : prod(n_windows)
+ [pk_loc,pk_mag(kk),sx,sy] = gauss2d_peaklocate(R_AB_norm(:,:,kk));
+ uv_shift(:,kk) = duv * (pk_loc - (wsize(:)/2 + 1));
+ sig_fit(1,kk) = sx;
+ sig_fit(2,kk) = sy;
+end
+u_shift = squeeze(uv_shift(1,:,:));
+v_shift = squeeze(uv_shift(2,:,:));
+pl_sigma = diag([du dv]) * reshape(sig_fit, [2 prod(n_windows)]);
+pl_sigma = reshape(pl_sigma, [2 n_windows]);
+
+%% map correspondences to image coordinates
+% define mask
+b_mask = true(n_windows);
+b_mask(:,[1:3 end]) = false;
+%b_mask([1 end],:) = false;
+
+% centers on reconstruction plane
+[um, vm] = ndgrid(u_ctr, v_ctr);
+uv_ctr = [um(:) vm(:)].';
+% correct point correspondences
+uv_ctr_A = uv_ctr + 1/2 *[uv_shift(1,:); uv_shift(2,:)];
+uv_ctr_B = uv_ctr - 1/2 *[uv_shift(1,:); uv_shift(2,:)];
+% project to image plane
+n_vec = prod(n_windows);
+gl_ctr = P_0 * [uv_ctr; zeros(1,n_vec); ones(1,n_vec)];
+gl_ctr_A = P_0 * [uv_ctr_A; zeros(1,n_vec); ones(1,n_vec)];
+gl_ctr_B = P_0 * [uv_ctr_B; zeros(1,n_vec); ones(1,n_vec)];
+im_ctr_A = pinhole_model(gl_ctr_A, c_A);
+im_ctr_B = pinhole_model(gl_ctr_B, c_B);
+im_ctr = cat(3, im_ctr_A, im_ctr_B);
+% triangulate
+[gl_ctr_new,gl_dist,im_dist]=triangulate(s_model.camera, im_ctr, true);
+
+%% solve for new best fit plane in the least squares sense
+% excluding masked points
+ok = b_mask(:) & ~any(isnan(gl_ctr_new),1)';
+n_ok = nnz(ok);
+u_shift_rms = rms(u_shift(ok));
+v_shift_rms = rms(v_shift(ok));
+
+% solve
+%M_new = fit_plane(gl_ctr_new(:,ok), M(1:3));
+M_new = robust_fit_plane(gl_ctr_new(:,ok));
+% preserve sense of orientation of sheet
+M_new = M_new * sign(dot(M_new(1:3), M(1:3)));
+
+%% extract rotation of laser sheet
+alpha = acosd(dot(M_new(1:3), M(1:3)));
+e_rot = cross(M(1:3), M_new(1:3));
+e_rot = e_rot / norm(e_rot);
+dz = M_new(4) - M(4);
+
+%% measure new residual
+% define new coordinate system
+[pl_to_gl_new, x_0_new] = define_planar_coord_system(M_new, [0 1 0]');
+% back-project point correspondences onto new measurement plane
+gl_bp_A = back_project_1c(c_A, M_new, im_ctr_A, true);
+gl_bp_B = back_project_1c(c_B, M_new, im_ctr_B, true);
+% convert to plane coordinate system
+pl_bp_A = pl_to_gl_new \ bsxfun(@minus, gl_bp_A, x_0_new);
+pl_bp_B = pl_to_gl_new \ bsxfun(@minus, gl_bp_B, x_0_new);
+% measure disparity in plane
+pl_disparity = pl_bp_A - pl_bp_B;
+u_new_rms = rms(pl_disparity(1,ok));
+v_new_rms = rms(pl_disparity(2,ok));
+%w_new_rms = rms(pl_disparity(3,ok));
+
+%% corners of new fit plane
+[uc,vc,wc] = ndgrid(u_axis([1 end]), v_axis([1 end]), 0);
+gl_corners = pl_to_gl_new*[uc([1 2 4 3]); vc([1 2 4 3]); wc([1 2 4 3])] + x_0_new;
+
+%% save laser sheet measurement in structure
+sheet = struct();
+sheet.gl_pos = gl_ctr_new(:,ok); % position of points on sheet
+sheet.pl_sigma = pl_sigma(:,ok); % peak width
+sheet.pl_to_gl = pl_to_gl_new; % orientation of sheet
+sheet.gl_origin = x_0_new; % origin of sheet
+
+% point correspondences
+sheet.im_pos_A = im_ctr_A(:,ok); % points on image A
+sheet.im_pos_B = im_ctr_B(:,ok); % points on image B
+
+%% report
+fprintf('------------------------------------------------------------------\n');
+fprintf('SELF CALIBRATION COMPLETE\n\n');
+fprintf('RMS disparity:\n');
+fprintf(' before:\n');
+fprintf(' u = %0.3f mm [%0.3f px]\n', u_shift_rms, u_shift_rms / du);
+fprintf(' v = %0.3f mm [%0.3f px]\n', v_shift_rms, v_shift_rms / dv);
+fprintf(' after:\n');
+fprintf(' u = %0.3f mm [%0.3f px]\n', u_new_rms, u_new_rms / du);
+fprintf(' v = %0.3f mm [%0.3f px]\n', v_new_rms, v_new_rms / dv);
+
+fprintf('Old plane:\n');
+fprintf(' normal %6.3f %6.3f %6.3f\n', M(1:3));
+fprintf(' shift %6.3f mm\n', M(4));
+fprintf('New plane:\n');
+fprintf(' normal %6.3f %6.3f %6.3f\n', M_new(1:3));
+fprintf(' shift %6.3f mm\n', M_new(4));
+fprintf('Rotation:\n');
+fprintf(' %6.3f degrees about %6.3f %6.3f %6.3f\n', alpha, e_rot);
+fprintf('Shift:\n');
+fprintf(' %6.3f mm\n', M_new(4)-M(4));
+
+%% plot plane fit
+if b_interactive
+ figure(10);
+ clf;
+ plot3(gl_ctr(1,:), gl_ctr(2,:), gl_ctr(3,:), 'rx', 'DisplayName', 'old');
+
+ hold on;
+ plot3(gl_ctr_new(1,:), gl_ctr_new(2,:), gl_ctr_new(3,:), 'k.', 'DisplayName', 'new');
+ plot3(gl_ctr_new(1,ok), gl_ctr_new(2,ok), gl_ctr_new(3,ok), 'ko', 'DisplayName', 'new');
+
+ xl = xlim;
+ yl = ylim;
+
+
+ %x_fit = xl([1 1 2 2]);
+ %y_fit = yl([1 2 2 1]);
+ %z_fit = (-M_new(4) - M_new(1)*x_fit - M_new(2)*y_fit)/M_new(3);
+ patch(gl_corners(1,:),gl_corners(2,:),gl_corners(3,:),1,'facecolor', 'b', 'facealpha', 0.5);
+end
+
+%% plot correlation
+if b_interactive
+ figure(1);
+ clf;
+ R_AB_mosaic = mosaic(R_AB_norm);
+ imagesc(stereo.u(ir), stereo.v(jr), R_AB_mosaic');
+ axis equal tight xy
+ set(gca,'clim', [0 0.08]);
+ title('Mean of image correlation');
+end
+
+%% plot sample correlation
+if b_interactive
+ figure(2);
+ clf;
+ ii = floor(n_windows(1)/2);
+ jj = floor(n_windows(2)/2);
+ C = R_AB_norm(:,:,ii,jj);
+
+ du_ax = du*((0:wsize(1)-1)-wsize(1)/2);
+ dv_ax = dv*((0:wsize(2)-1)-wsize(2)/2);
+ [dum,dvm]= ndgrid(du_ax,dv_ax);
+
+ plot3(dum, dvm, C, 'k.');
+ %surf(du_ax, dv_ax, C');
+
+ title('Correlation at center of image');
+ view(3);
+
+ [~,~,~,~,C_fit] = gauss2d_peaklocate(C);
+
+ hold on;
+ S_fit = surf(dum', dvm', C_fit');
+ set(S_fit, 'facealpha', 0.5, 'edgecolor', 'none');
+end
+
+%% display dewarped images
+if b_interactive
+ figure(3);
+ clf;
+ subplot(1,2,1);
+ ax = gca;
+ imagesc(stereo.u, stereo.v, pl_imA');
+ axis equal xy;
+ xlim(stereo.u([1 end]));
+ ylim(stereo.v([1 end]));
+ title('Camera 1, dewarped');
+ %ylim(j_axis([end-512 end]));
+ %xlim(i_axis([1 end]));
+ hold on;
+ plot(um(:), vm(:), 'ro');
+ quiversc(um, vm, u_shift, v_shift, 10, 'r-');
+
+ subplot(1,2,2);
+ ax(2)=gca;
+ imagesc(stereo.u, stereo.v, pl_imB');
+ axis equal xy;
+ xlim(stereo.u([1 end]));
+ ylim(stereo.v([1 end]));
+ title('Camera 2, dewarped');
+ hold on;
+ plot(um+u_shift, vm+v_shift, 'ro');
+
+ %quiver(um, vm,-squeeze(uv_shift(1,:,:)),-squeeze(uv_shift(2,:,:)), 'r-');
+
+ linkaxes(ax, 'xy');
+end
+
+%% plot raw images
+if b_interactive
+ figure(4);
+ clf;
+ subplot(1,2,1);
+ imagesc(i_axis, j_axis, imA');
+ axis equal tight ij;
+ hold on;
+ plot(im_ctr_A(1,:), im_ctr_A(2,:), 'ro');
+ title('Camera 2, raw');
+
+ subplot(1,2,2);
+ imagesc(i_axis, j_axis, imB');
+ axis equal tight ij;
+ hold on;
+ plot(im_ctr_B(1,:), im_ctr_B(2,:), 'ro');
+ title('Camera 1, raw');
+end
+
+%% save result to file
+b_save = true;
+if b_interactive
+ b_save = input('Save new calibration to file? y/N ', 's');
+ b_save = strcmpi(b_save, 'Y');
+end
+
+if b_save
+ %% build new model and save
+ fprintf('Saving %s\n', cal_file);
+
+ % store
+ s_model.pl_to_gl = pl_to_gl_new;
+ s_model.gl_origin = x_0_new;
+ s_model.sheet = sheet;
+ % update self-calibration counter
+ if isfield(s_model, 'n_selfcal_iter')
+ s_model.n_selfcal_iter = s_model.n_selfcal_iter + 1;
+ else
+ s_model.n_selfcal_iter = 1;
+ end
+
+ save(cal_file, '-struct', 's_model');
+end
\ No newline at end of file
diff --git a/stereo/spiv_selfcal_camera.m b/stereo/spiv_selfcal_camera.m
new file mode 100644
index 0000000000000000000000000000000000000000..e3194faaa33978c51bad6740865cb25c15befa82
--- /dev/null
+++ b/stereo/spiv_selfcal_camera.m
@@ -0,0 +1,256 @@
+%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
+% manual self-calibration for correction of camera model
+%
+% Author: John Lawson
+% j.m.lawson@soton.ac.uk
+% Created: 04/07/2019
+%
+
+% path of MATLAB calibration file
+cal_dir = 'H:\davis\multiscale\Gen1_123\U10\U10_x-axis=632.986_x-axis=632.985_x-axis=604.987';
+cal_pat = '%s/plate_900.mat';
+cal_pat_new = '%s/plate_900.rotated.mat';
+cal_file = sprintf(cal_pat, cal_dir);
+cal_file_new = sprintf(cal_pat_new, cal_dir);
+
+% plot options
+c_lim = [0 512];
+y_lim = [4871-750 4871];
+font_size = 24;
+
+%% load camera model
+fprintf('------------------------------------------------------------------\n');
+fprintf('loading camera model\n');
+s_model = importdata(cal_file);
+i_axis = s_model.camera(1).i_axis;
+j_axis = s_model.camera(1).j_axis;
+cmodel = s_model.camera;
+c_A = cmodel(1).calib;
+c_B = cmodel(2).calib;
+
+im_size = [length(i_axis) length(j_axis)];
+
+% sheet model
+pl_to_gl = s_model.sheet.pl_to_gl;
+x_0 = s_model.sheet.gl_origin;
+M = [pl_to_gl(:,3); -dot(x_0, pl_to_gl(:,3))]';
+
+% point correspondnces on sheet
+im_pos_A = s_model.sheet.im_pos_A;
+im_pos_B = s_model.sheet.im_pos_B;
+gl_old = s_model.sheet.gl_pos;
+im_pos = cat(3, im_pos_A, im_pos_B);
+n_points = size(im_pos_A,2);
+
+pl_corners = pl_to_gl^-1 * (gl_old - s_model.sheet.gl_origin);
+u_lim = [min(pl_corners(1,:)) max(pl_corners(1,:))];
+v_lim = [min(pl_corners(2,:)) max(pl_corners(2,:))];
+
+%% test model
+[gl_pos,gl_dist,im_dist]= triangulate(cmodel, im_pos, true);
+d = M * [gl_pos; ones(1, n_points)];
+d_max = max(abs(d));
+d_rms = rms(d);
+
+fprintf('before self-calibration:\n');
+fprintf(' rms triangulation error: %6.3f mm\n', rms(gl_dist(:)));
+fprintf(' in camera 1: %6.3f px\n', rms(im_dist(:,1)));
+fprintf(' in camera 2: %6.3f px\n', rms(im_dist(:,2)));
+fprintf(' max deviation: %6.3f mm\n', d_max);
+fprintf(' rms deviation: %6.3f mm\n', d_rms);
+
+%% optimise rotation of first camera to match observation
+% we try to minimise the triangulation error by adjusting the rotation
+% matrix of one camera
+X0 = [0 0 0] * eps;
+Gamma = 1E3; % regularisation factor
+cfun = @(X) triangulation_cost(X, cmodel, im_pos, true, Gamma);
+X = lsqnonlin(cfun, X0);
+
+%% make new model
+[~,newmodel] = triangulation_cost(X, cmodel, im_pos, true, Gamma);
+
+% extract rotation
+R1p = newmodel(1).calib.R;
+R2p = newmodel(2).calib.R;
+Omega1 = R1p * cmodel(1).calib.R^-1;
+Omega2 = R2p * cmodel(2).calib.R^-1;
+omega1 = vrrotmat2vec(Omega1);
+omega2 = vrrotmat2vec(Omega2);
+e1 = omega1(1:3);
+theta1 = omega1(4);
+e2 = omega2(1:3);
+theta2 = omega2(4);
+% extract shift
+dx1 = newmodel(1).calib.xc - cmodel(1).calib.xc;
+dx2 = newmodel(2).calib.xc - cmodel(2).calib.xc;
+
+% relative rotation of cameras
+R12 = R2p * R1p^-1;
+r12 = vrrotmat2vec(R12);
+theta12 = r12(4);
+e12 = r12(1:3) / norm(r12(1:3));
+
+% fit new model of laser sheet
+[gl_pos,gl_dist,im_dist]= triangulate(newmodel, im_pos, true);
+M_new = fit_plane(gl_pos);
+% ensure sense is same
+M_new = M_new * sign(dot(M_new(1:3), M(1:3)));
+
+[pl_to_gl_new, x_0_new] = define_planar_coord_system(M_new, [0 1 0]');
+dp = M_new * [gl_pos; ones(1, n_points)];
+dp_max = max(abs(dp));
+dp_rms = rms(d);
+
+%% corners of new fit plane
+[uc,vc,wc] = ndgrid(u_lim([1 end]), v_lim([1 end]), 0);
+gl_corners = pl_to_gl_new*[uc([1 2 4 3]); vc([1 2 4 3]); wc([1 2 4 3])] + x_0_new;
+
+%% test new model
+fprintf('after self-calibration:\n');
+fprintf(' C1 rotates %8.3f deg about %6.3f %6.3f %6.3f\n', theta1/pi*180, e1);
+fprintf(' C2 rotates %8.3f deg about %6.3f %6.3f %6.3f\n', theta2/pi*180, e2);
+fprintf(' C1->C2 %8.3f deg about %6.3f %6.3f %6.3f\n', theta12/pi*180, e12);
+fprintf(' C1 translates %6.3f %6.3f %6.3f mm\n', dx1);
+fprintf(' C2 translates %6.3f %6.3f %6.3f mm\n', dx2);
+fprintf(' sheet normal %6.3f %6.3f %6.3f\n', M_new(1:3));
+fprintf(' sheet shift %6.3f mm\n', M_new(4));
+fprintf('\n');
+fprintf(' rms triangulation error: %6.3f mm\n', rms(gl_dist(:)));
+fprintf(' in camera 1: %6.3f px\n', rms(im_dist(:,1)));
+fprintf(' in camera 2: %6.3f px\n', rms(im_dist(:,2)));
+
+fprintf(' max deviation: %6.3f mm\n', dp_max);
+fprintf(' rms deviation: %6.3f mm\n', dp_rms);
+
+%% plot plane fit
+figure(10);
+clf;
+% current points
+plot3(gl_pos(1,:), gl_pos(2,:), gl_pos(3,:), 'rx');
+hold on;
+% old estimate of points
+plot3(gl_old(1,:), gl_old(2,:), gl_old(3,:), 'ko');
+patch(gl_corners(1,:),gl_corners(2,:),gl_corners(3,:),1,'facecolor', 'b', 'facealpha', 0.5);
+daspect(1 ./ (M_new(1:3)+0.05));
+xlabel('x');
+ylabel('y');
+zlabel('z');
+
+%% save
+fprintf('save to %s\n', cal_file_new);
+s_model.camera = newmodel;
+s_model.sheet.gl_pos = gl_pos;
+s_model.sheet.pl_to_gl = pl_to_gl_new;
+s_model.sheet.gl_origin = x_0_new;
+s_model.pl_to_gl = pl_to_gl_new;
+s_model.gl_origin = x_0_new;
+save(cal_file_new, '-struct', 's_model');
+
+%{
+error('stop');
+
+%% convert to canonical camera model
+R12 = cmodel(2).calib.R * R1p^-1;
+x12 = cmodel(2).calib.xc - cmodel(1).calib.xc;
+
+rmodel = newmodel;
+rmodel(1).calib.xc = [0 0 0]';
+rmodel(2).calib.xc = R1p * x12;
+rmodel(1).calib.R = eye(3);
+rmodel(2).calib.R = R12;
+
+rmodel(1).calib.P = rmodel(1).calib.G * [rmodel(1).calib.R -rmodel(1).calib.xc];
+rmodel(2).calib.P = rmodel(2).calib.G * [rmodel(2).calib.R -rmodel(2).calib.xc];
+
+%%
+
+% the camera chosen should not matter, since
+
+% see Hartley & Zisserman chapters 9-11
+
+% get intrinsic parameters from camera calibration
+G_A = c_A.G;
+G_B = c_B.G;
+% correct for radial distortion
+dw_pos_A = inverse_radial_distortion(im_pos_A, c_A.ic, c_A.kr);
+dw_pos_B = inverse_radial_distortion(im_pos_B, c_B.ic, c_B.kr);
+
+% compute essential matrix E
+cpA = cameraParameters('IntrinsicMatrix', G_A');
+cpB = cameraParameters('IntrinsicMatrix', G_B');
+[E,inliers,status] = estimateEssentialMatrix(dw_pos_A.', dw_pos_B.', cpA, cpB, 'MaxDistance', 1);
+
+% compute
+F = estimateFundamentalMatrix(dw_pos_A.', dw_pos_B.');
+Ep = G_B.' * F * G_A;
+
+fprintf('Computed essential matrix, status %d, %d / %d inliers\n', status, nnz(inliers), n_points);
+[U,D,V] = svd(E);
+d = diag(D);
+assert(norm(d - [1 1 0]') < 1E-10); % svds of E should be 1,1,0 (approx)
+
+PA = [eye(3), [0;0;0]];
+% following Hartley &Zisserman section 9.6.2, p258
+% extract the canonical rotation matrix and camera baseline from the
+% essential matrix
+
+W = [0 -1 0; 1 0 0; 0 0 1]; % (9.13)
+Z = [0 1 0; -1 0 0; 0 0 0]; % (9.13)
+S = U*Z*U.'; % (9.14)
+R1 = U*W*V.'; % (9.14)
+R2 = U*(W.')*(V.'); % (9.14)
+
+
+%% load image
+fprintf('------------------------------------------------------------------\n');
+fprintf('loading images ...\n');
+im_min = arrayfun(@(x) inf(im_size), 1:n_frames, 'uniformoutput', false);
+im_max = arrayfun(@(x) zeros(im_size), 1:n_frames, 'uniformoutput', false);
+im_mean = arrayfun(@(x) zeros(im_size), 1:n_frames, 'uniformoutput', false);
+im_ms = arrayfun(@(x) zeros(im_size), 1:n_frames, 'uniformoutput', false);
+
+for n = 1 : n_src
+ %% load image
+ src_name = src_fun(src_range(n));
+ fprintf('[%3d of %3d] %s ...\n', n, n_src, src_name);
+ imx = readimx(src_name);
+ im_ary = cellfun(@(f) double(f.Components{1}.Planes{1}), imx.Frames, 'uniformoutput', false);
+
+ for f = 1 : n_frames
+ im_min{f} = min(im_ary{f}, im_min{f});
+ im_max{f} = max(im_ary{f}, im_max{f});
+ im_mean{f} = im_mean{f} + im_ary{f} / n_src;
+ im_ms{f} = im_ms{f} + im_ary{f}.^2 / n_src;
+ end
+end
+
+%% select min/max/mean/std
+imA = im_min{1};
+imB = im_min{3};
+
+%% draw
+figure(1);
+clf;
+subplot(2,1,1);
+ax=gca;
+imagesc(i_axis, j_axis, imA');
+axis equal tight ij;
+hold on;
+plot(im_pos_A(1,:), im_pos_A(2,:), 'ro');
+ylim(y_lim);
+set(gca, 'clim', c_lim);
+title('Camera 1', 'fontsize', font_size);
+
+
+subplot(2,1,2);
+imagesc(i_axis, j_axis, imB');
+ax(2)=gca;
+axis equal tight ij;
+hold on;
+plot(im_pos_B(1,:), im_pos_B(2,:), 'ro');
+ylim(y_lim);
+
+set(gca, 'clim', c_lim);
+title('Camera 2', 'fontsize', font_size);
+%}
\ No newline at end of file
diff --git a/stereo/spiv_selfcal_manual.m b/stereo/spiv_selfcal_manual.m
new file mode 100644
index 0000000000000000000000000000000000000000..cfc79844668b8c748f446b00f9efcc5b26652036
--- /dev/null
+++ b/stereo/spiv_selfcal_manual.m
@@ -0,0 +1,167 @@
+%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
+% manual self-calibration for correction of camera model
+%
+% Author: John Lawson
+% j.m.lawson@soton.ac.uk
+% Created: 04/07/2019
+%
+
+% sample images for self calibration
+src_dir = 'H:\davis\multiscale\Gen1\U10\U10_x-axis=316.313_x-axis=316.32_x-axis=288.317';
+src_pat = '%s/B%05d.im7';
+src_range = 1 : 50 : 1000;
+src_fun = @(n) sprintf(src_pat, src_dir, n);
+n_src = length(src_range);
+n_frames = 4;
+i_cam = 1;
+
+% path of MATLAB calibration file
+cal_dir = src_dir;
+cal_pat = '%s/pinhole.rotated.mat';
+cal_file = sprintf(cal_pat, cal_dir);
+
+% plot options
+c_lim = [0 512];
+y_lim = [4871-750 4871];
+font_size = 24;
+
+%% load camera model
+fprintf('------------------------------------------------------------------\n');
+fprintf('loading model for camera %d\n', i_cam);
+s_model = importdata(cal_file);
+i_axis = s_model.camera(i_cam).i_axis;
+j_axis = s_model.camera(i_cam).j_axis;
+cmodel = s_model.camera(i_cam).calib;
+im_size = [length(i_axis) length(j_axis)];
+
+%% load image
+fprintf('------------------------------------------------------------------\n');
+fprintf('loading images ...\n');
+im_min = arrayfun(@(x) inf(im_size), 1:n_frames, 'uniformoutput', false);
+im_max = arrayfun(@(x) zeros(im_size), 1:n_frames, 'uniformoutput', false);
+im_mean = arrayfun(@(x) zeros(im_size), 1:n_frames, 'uniformoutput', false);
+im_ms = arrayfun(@(x) zeros(im_size), 1:n_frames, 'uniformoutput', false);
+
+for n = 1 : n_src
+ %% load image
+ src_name = src_fun(src_range(n));
+ fprintf('[%3d of %3d] %s ...\n', n, n_src, src_name);
+ imx = readimx(src_name);
+ im_ary = cellfun(@(f) double(f.Components{1}.Planes{1}), imx.Frames, 'uniformoutput', false);
+
+ for f = 1 : n_frames
+ im_min{f} = min(im_ary{f}, im_min{f});
+ im_max{f} = max(im_ary{f}, im_max{f});
+ im_mean{f} = im_mean{f} + im_ary{f} / n_src;
+ im_ms{f} = im_ms{f} + im_ary{f}.^2 / n_src;
+ end
+end
+
+%% select min/max/mean/std
+imA = im_min{(i_cam-1)*2+1};
+imB = s_model.camera(i_cam).calib.im;
+% prefilter
+wsize = [15 15];
+G_log = fspecial('log', wsize, wsize(1)/4);
+imA = conv2(imA, G_log, 'same');
+imB = conv2(imB, G_log, 'same');
+
+%% draw
+figure(1);
+clf;
+subplot(2,1,1);
+ax=gca;
+imagesc(i_axis, j_axis, imA');
+axis equal tight ij;
+ylim(y_lim);
+%set(gca, 'clim', c_lim);
+set(gca, 'clim', [-1 1]*0.5);
+
+title('Calibration image', 'fontsize', font_size);
+hold on;
+
+subplot(2,1,2);
+imagesc(i_axis, j_axis, imB');
+ax(2)=gca;
+axis equal tight ij;
+ylim(y_lim);
+hold on;
+%set(gca, 'clim', c_lim);
+set(gca, 'clim', [-1 1]*0.5);
+title('Measurement', 'fontsize', font_size);
+
+%% prompt user to identify point correspondences
+fprintf('------------------------------------------------------------------\n');
+fprintf('Draw pairs of point correspondences\n');
+fprintf('Left click to add correspondences\n');
+fprintf('Right click to stop\n');
+
+imA_pts = [];
+imB_pts = [];
+n_points = 0;
+delete(findobj(ax,'type','text'));
+
+while true
+ %% get a pair of correspondences
+ % camera 1
+ axes(ax(1)); %#ok<LAXES>
+ [x1,y1,button] = ginput(1);
+ if button ~= 1
+ break;
+ end
+ text(x1, y1, num2str(n_points+1));
+ plot(x1, y1, 'ko');
+
+ % camera 2
+ axes(ax(2)); %#ok<LAXES>
+ [x2,y2,button] = ginput(1);
+ if button ~= 1
+ break;
+ end
+ text(x2, y2, num2str(n_points+1));
+ plot(x2, y2, 'ko');
+
+ %% add to array
+ n_points = n_points + 1;
+ imA_pts = [imA_pts, [x1;y1]]; %#ok<AGROW>
+ imB_pts = [imB_pts, [x2;y2]]; %#ok<AGROW>
+ fprintf('[%d] (%0.1f, %0.1f) <-> (%0.1f, %0.1f)\n', n_points, x1, y1, x2, y2);
+end
+
+%% report
+fprintf('identified %d correspondences\n', n_points);
+fprintf('shift:\n');
+dij = imB_pts - imA_pts;
+for n = 1 : n_points
+ fprintf('[%d] %6.3f, %6.3f px\n', n, dij(:,n));
+end
+
+%% measure unit vectors
+G = cmodel.G;
+y_ref_tilde = [imA_pts; ones(1, n_points)];
+y_rot_tilde = [imB_pts; ones(1, n_points)];
+r_ref = unitvector(G \ y_ref_tilde);
+r_rot = unitvector(G \ y_rot_tilde);
+
+sint0 = sqrt(sum(cross(r_rot, r_ref, 1).^2, 1));
+
+[e_rot, sint] = unitvector(mean(cross(r_rot, r_ref, 1), 2));
+theta = asin(sint);
+Omega = vrrotvec2mat([e_rot; theta]);
+
+cost_residual = dot(r_rot, Omega * r_ref, 1);
+theta_residual = acos(cost_residual);
+theta_rms = rms(theta_residual);
+
+fprintf('rotate %6.3f deg about %6.3f %6.3f %6.3f\n', theta/pi*180, e_rot);
+
+%% modify camera model
+cmodel.R = Omega * cmodel.R;
+cmodel.P = cmodel.G * cmodel.R * [eye(3), -cmodel.xc];
+s_model.camera(i_cam).calib = cmodel;
+
+s = input('Write new calibration to file? [y/N]', 's');
+b_ok = strcmpi(s, 'y');
+if b_ok
+ save(cal_file, '-struct', 's_model');
+end
diff --git a/stereo/spiv_selfcal_manual_shift.m b/stereo/spiv_selfcal_manual_shift.m
new file mode 100644
index 0000000000000000000000000000000000000000..9f187017c058aa47368fe0dd9f309e86266c1fd0
--- /dev/null
+++ b/stereo/spiv_selfcal_manual_shift.m
@@ -0,0 +1,149 @@
+%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
+% manual self-calibration for correction of camera model
+%
+% Author: John Lawson
+% j.m.lawson@soton.ac.uk
+% Created: 04/07/2019
+%
+
+calib_dir = 'H:\davis\multiscale\Properties\Calibration History\Gen1_mid_33.33\';
+
+% sample images for self calibration
+src_dir = 'H:\davis\multiscale\Gen1\U10\U10_x-axis=257.986_x-axis=257.985_x-axis=229.987\';
+src_pat = '%s/B%05d.im7';
+src_range = 1 : 50 : 1000;
+src_fun = @(n) sprintf(src_pat, src_dir, n);
+n_src = length(src_range);
+n_frames = 4;
+% path of MATLAB calibration file
+cal_dir = src_dir;
+cal_pat = '%s/pinhole.mat';
+cal_file = sprintf(cal_pat, cal_dir);
+
+% plot options
+c_lim = [0 512];
+y_lim = [4871-750 4871];
+font_size = 24;
+
+%% load camera model
+fprintf('loading camera model ...\n');
+s_model = importdata(cal_file);
+i_axis = s_model.camera(1).i_axis;
+j_axis = s_model.camera(1).j_axis;
+c_A = s_model.camera(1).calib;
+c_B = s_model.camera(2).calib;
+im_size = [length(i_axis) length(j_axis)];
+
+%% load image
+fprintf('loading images ...\n');
+im_min = arrayfun(@(x) inf(im_size), 1:n_frames, 'uniformoutput', false);
+im_max = arrayfun(@(x) zeros(im_size), 1:n_frames, 'uniformoutput', false);
+im_mean = arrayfun(@(x) zeros(im_size), 1:n_frames, 'uniformoutput', false);
+im_ms = arrayfun(@(x) zeros(im_size), 1:n_frames, 'uniformoutput', false);
+
+for n = 1 : n_src
+ %% load image
+ src_name = src_fun(src_range(n));
+ fprintf('[%3d of %3d] %s ...\n', n, n_src, src_name);
+ imx = readimx(src_name);
+ im_ary = cellfun(@(f) double(f.Components{1}.Planes{1}), imx.Frames, 'uniformoutput', false);
+
+ for f = 1 : n_frames
+ im_min{f} = min(im_ary{f}, im_min{f});
+ im_max{f} = max(im_ary{f}, im_max{f});
+ im_mean{f} = im_mean{f} + im_ary{f} / n_src;
+ im_ms{f} = im_ms{f} + im_ary{f}.^2 / n_src;
+ end
+end
+
+%% process
+fprintf('processing ...\n');
+wsize = [33 33];
+G = ones(wsize)/prod(wsize);
+
+im_var = cell(1,n_frames);
+im_std = cell(1,n_frames);
+
+for f = 1 : n_frames
+ im_var{f} = max(0,im_ms{f} - im_mean{f}.^2);
+ im_std{f} = sqrt(im_var{f});
+end
+
+%% select min/max/mean/std
+imA = im_min{1};
+imB = s_model.camera(1).calib.im;
+
+%% draw
+figure(1);
+clf;
+subplot(2,1,1);
+ax=gca;
+imagesc(i_axis, j_axis, imA');
+axis equal tight ij;
+ylim(y_lim);
+set(gca, 'clim', c_lim);
+title('Camera 1', 'fontsize', font_size);
+hold on;
+
+subplot(2,1,2);
+imagesc(i_axis, j_axis, imB');
+ax(2)=gca;
+axis equal tight ij;
+ylim(y_lim);
+hold on;
+set(gca, 'clim', c_lim);
+title('Camera 2', 'fontsize', font_size);
+
+%% prompt user to identify point correspondences
+fprintf('------------------------------------------------------------------\n');
+fprintf('Draw pairs of point correspondences\n');
+fprintf('Left click to add correspondences\n');
+fprintf('Right click to stop\n');
+
+imA_pts = [];
+imB_pts = [];
+n_points = 0;
+delete(findobj(ax,'type','text'));
+
+while true
+ %% get a pair of correspondences
+ % camera 1
+ axes(ax(1)); %#ok<LAXES>
+ [x1,y1,button] = ginput(1);
+ if button ~= 1
+ break;
+ end
+ text(x1, y1, num2str(n_points+1));
+ plot(x1, y1, 'ko');
+
+ % camera 2
+ axes(ax(2)); %#ok<LAXES>
+ [x2,y2,button] = ginput(1);
+ if button ~= 1
+ break;
+ end
+ text(x2, y2, num2str(n_points+1));
+ plot(x2, y2, 'ko');
+
+ %% add to array
+ n_points = n_points + 1;
+ imA_pts = [imA_pts, [x1;y1]];
+ imB_pts = [imB_pts, [x2;y2]];
+ fprintf('[%d] (%0.1f, %0.1f) <-> (%0.1f, %0.1f)\n', n_points, x1, y1, x2, y2);
+end
+
+%% triangulate
+if n_points < 2
+ error('Too few points selected to proceed');
+end
+im_pts = cat(3, imA_pts, imB_pts);
+
+% triangulate
+[gl_pts, gl_dist, ij_dist] = triangulate(s_model.camera, im_pts);
+% reproject
+im_proj_A = pinhole_model(gl_pts, s_model.camera(1).calib, true);
+im_proj_B = pinhole_model(gl_pts, s_model.camera(2).calib, true);
+
+% plot
+plot(ax(1), im_proj_A(1,:), im_proj_A(2,:), 'ro');
+plot(ax(2), im_proj_B(1,:), im_proj_B(2,:), 'ro');
diff --git a/stereo/triangulation_cost.m b/stereo/triangulation_cost.m
new file mode 100644
index 0000000000000000000000000000000000000000..28acde07cc3fd8b242b6f2db707b7bedab3a0973
--- /dev/null
+++ b/stereo/triangulation_cost.m
@@ -0,0 +1,42 @@
+function [f_cost, camera] = triangulation_cost(X, camera, ij_pos, b_dewarp, Gamma)
+ % extract rotation, camera 1
+ e1 = X(1:3) / (eps + norm(X(1:3)));
+ theta1 = norm(X(1:3));
+ % extract rotation, camera 2
+ e2 = e1;
+ theta2 = -theta1;
+ % extract shift, camera 1
+ dx1 = [0 0 0]';
+ dx2 = [0 0 0]';
+
+ % create rotation matrices
+ Omega1 = vrrotvec2mat([e1, theta1]);
+ Omega2 = vrrotvec2mat([e2, theta2]);
+
+ % apply rotation to cameras
+ R1p = Omega1 * camera(1).calib.R;
+ R2p = Omega2 * camera(2).calib.R;
+ camera(1).calib.R = R1p;
+ camera(2).calib.R = R2p;
+
+ % apply translation
+ x1p = camera(1).calib.xc + dx1;
+ x2p = camera(2).calib.xc + dx2;
+ camera(1).calib.xc= x1p;
+ camera(2).calib.xc= x2p;
+
+ % update camera projection matrix
+ camera(1).calib.P = camera(1).calib.G * camera(1).calib.R * [eye(3), -camera(1).calib.xc];
+ camera(2).calib.P = camera(2).calib.G * camera(2).calib.R * [eye(3), -camera(2).calib.xc];
+
+ % triangulate
+ [gl_pos,gl_dist]= triangulate(camera, ij_pos, b_dewarp);
+ f_cost = gl_dist(:,1);
+
+ % fit plane
+ M = fit_plane(gl_pos, [0 1 0]');
+ d = M * [gl_pos; ones(1,size(gl_pos,2))];
+
+ % add regularisation factor
+ f_cost = [f_cost; d'; Gamma*theta1];
+end
\ No newline at end of file