update of pinhole fitting procedure and some auxiliary functions

misc/parsave.m

+function parsave(fname, s, varargin)
+	save(fname, '-struct', 's', varargin{:});
+end
\ No newline at end of file

misc/retrieve_marker_coord.m

+function [Coord,corners_cam] = retrieve_marker_coord(calibname)
+
+% This function opens a DaVis 'MarkPositionTable.xml' file and retrieves the
+% the position of markers. In raw image coordinate system (Image plane), 
+% [X,Y] coordinates are given in pixels while in the world coordinate system
+% (Object plane), [x,y,z] coordinates are in mm. This is then returned in 
+% two matrices corresponding to each camera. This script uses 'xml2struct'.  
+% It must be searchable in the MATLAB path [Download link:
+% https://uk.mathworks.com/matlabcentral/fileexchange/28518-xml2struct]
+% 
+% INPUT VARIABLES:
+% filename = pathname of xml file
+% 
+% OUTPUT VARIABLES:
+% Coord       = Image and object plane coordinates for left and right camera
+%               Format: 1st column - image plane x coordinates [in pixels]
+%                       2nd column - image plane y coordinates [in pixels]
+%                       3rd column - object plane x coordinates [in mm]
+%                       4th column - object plane y coordinates [in mm]
+%                       5th column - objetc plane z coordinates [in mm]
+% corners_cam = The pixels of the camera's corners
+%
+%--------------------------------------------------------------------------
+% Version 1.0
+% Girish K. Jankee (26 January 2018)
+% 
+% Version 2.0
+% Eduardo Rodriguez-Lopez (24 April 2018)
+% Determination of distance between 2 z-planes is now automated.
+% Version 2.1
+% Girish K. Jankee (26 April 2018)
+% Added feature extracts the coordinates of the camera's corners.
+%--------------------------------------------------------------------------
+%% MAIN CODE
+a = xml2struct(calibname);
+for i = 1:2
+    if i == 1
+        cam_L = cell2mat(a.MarkTable(1).Camera{1,i}.View(1,1).Mark);
+    else
+        cam_R = cell2mat(a.MarkTable(1).Camera{1,i}.View(1,1).Mark);
+    end
+end
+
+% Left Camera
+posL = zeros(length(cam_L),5);
+for j = 1:length(cam_L)
+    posL(j,1) = str2num(cam_L(j).RawPos(1).Attributes(1).x);
+    posL(j,2) = str2num(cam_L(j).RawPos(1).Attributes(1).y);    
+    posL(j,3) = str2num(cam_L(j).WorldPos(1).Attributes(1).x);
+    posL(j,4) = str2num(cam_L(j).WorldPos(1).Attributes(1).y);    
+    posL(j,5) = str2num(cam_L(j).WorldPos(1).Attributes(1).z);
+end
+% keyboard
+thickL=abs(mean(posL( posL(:,5)~=0, 5 )));
+posL( abs(posL(:,5)+thickL)<1e-10, 5 ) = -thickL/2 ;
+posL( abs(posL(:,5)-0     )<1e-10, 5 ) =  thickL/2 ;
+
+% Right Camera
+posR = zeros(length(cam_R),5);
+for j = 1:length(cam_R)
+    posR(j,1) = str2num(cam_R(j).RawPos(1).Attributes(1).x);
+    posR(j,2) = str2num(cam_R(j).RawPos(1).Attributes(1).y);    
+    posR(j,3) = str2num(cam_R(j).WorldPos(1).Attributes(1).x);
+    posR(j,4) = str2num(cam_R(j).WorldPos(1).Attributes(1).y);    
+    posR(j,5) = str2num(cam_R(j).WorldPos(1).Attributes(1).z);
+end
+
+thickR=abs(mean(posR( posR(:,5)~=0, 5 )));
+posR( abs(posR(:,5)+thickR)<1e-10, 5 ) = -thickR/2 ;
+posR( abs(posR(:,5)-0     )<1e-10, 5 ) =  thickR/2 ;
+
+
+if thickR~=thickL
+    error('thickness are not the same!!!')
+end
+
+Coord.Left = posL;
+Coord.Right = posR;
+
+height = str2num(a.MarkTable(1).Camera{1,1}.Attributes.Height);
+width = str2num(a.MarkTable(1).Camera{1,1}.Attributes.Width);
+corners_cam    = [0 width width    0
+                  0    0 height height];          % the pixels of the camera's corners

misc/xml2struct.m

+function [ s ] = xml2struct( file )
+%Convert xml file into a MATLAB structure
+% [ s ] = xml2struct( file )
+%
+% A file containing:
+% <XMLname attrib1="Some value">
+%   <Element>Some text</Element>
+%   <DifferentElement attrib2="2">Some more text</Element>
+%   <DifferentElement attrib3="2" attrib4="1">Even more text</DifferentElement>
+% </XMLname>
+%
+% Will produce:
+% s.XMLname.Attributes.attrib1 = "Some value";
+% s.XMLname.Element.Text = "Some text";
+% s.XMLname.DifferentElement{1}.Attributes.attrib2 = "2";
+% s.XMLname.DifferentElement{1}.Text = "Some more text";
+% s.XMLname.DifferentElement{2}.Attributes.attrib3 = "2";
+% s.XMLname.DifferentElement{2}.Attributes.attrib4 = "1";
+% s.XMLname.DifferentElement{2}.Text = "Even more text";
+%
+% Please note that the following characters are substituted
+% '-' by '_dash_', ':' by '_colon_' and '.' by '_dot_'
+%
+% Written by W. Falkena, ASTI, TUDelft, 21-08-2010
+% Attribute parsing speed increased by 40% by A. Wanner, 14-6-2011
+% Added CDATA support by I. Smirnov, 20-3-2012
+%
+% Modified by X. Mo, University of Wisconsin, 12-5-2012
+
+    if (nargin < 1)
+        clc;
+        help xml2struct
+        return
+    end
+    
+    if isa(file, 'org.apache.xerces.dom.DeferredDocumentImpl') || isa(file, 'org.apache.xerces.dom.DeferredElementImpl')
+        % input is a java xml object
+        xDoc = file;
+    else
+        %check for existance
+        if (exist(file,'file') == 0)
+            %Perhaps the xml extension was omitted from the file name. Add the
+            %extension and try again.
+            if (isempty(strfind(file,'.xml')))
+                file = [file '.xml'];
+            end
+            
+            if (exist(file,'file') == 0)
+                error(['The file ' file ' could not be found']);
+            end
+        end
+        %read the xml file
+        xDoc = xmlread(file);
+    end
+    
+    %parse xDoc into a MATLAB structure
+    s = parseChildNodes(xDoc);
+    
+end
+
+% ----- Subfunction parseChildNodes -----
+function [children,ptext,textflag] = parseChildNodes(theNode)
+    % Recurse over node children.
+    children = struct;
+    ptext = struct; textflag = 'Text';
+    if hasChildNodes(theNode)
+        childNodes = getChildNodes(theNode);
+        numChildNodes = getLength(childNodes);
+
+        for count = 1:numChildNodes
+            theChild = item(childNodes,count-1);
+            [text,name,attr,childs,textflag] = getNodeData(theChild);
+            
+            if (~strcmp(name,'#text') && ~strcmp(name,'#comment') && ~strcmp(name,'#cdata_dash_section'))
+                %XML allows the same elements to be defined multiple times,
+                %put each in a different cell
+                if (isfield(children,name))
+                    if (~iscell(children.(name)))
+                        %put existsing element into cell format
+                        children.(name) = {children.(name)};
+                    end
+                    index = length(children.(name))+1;
+                    %add new element
+                    children.(name){index} = childs;
+                    if(~isempty(fieldnames(text)))
+                        children.(name){index} = text; 
+                    end
+                    if(~isempty(attr)) 
+                        children.(name){index}.('Attributes') = attr; 
+                    end
+                else
+                    %add previously unknown (new) element to the structure
+                    children.(name) = childs;
+                    if(~isempty(text) && ~isempty(fieldnames(text)))
+                        children.(name) = text; 
+                    end
+                    if(~isempty(attr)) 
+                        children.(name).('Attributes') = attr; 
+                    end
+                end
+            else
+                ptextflag = 'Text';
+                if (strcmp(name, '#cdata_dash_section'))
+                    ptextflag = 'CDATA';
+                elseif (strcmp(name, '#comment'))
+                    ptextflag = 'Comment';
+                end
+                
+                %this is the text in an element (i.e., the parentNode) 
+                if (~isempty(regexprep(text.(textflag),'[\s]*','')))
+                    if (~isfield(ptext,ptextflag) || isempty(ptext.(ptextflag)))
+                        ptext.(ptextflag) = text.(textflag);
+                    else
+                        %what to do when element data is as follows:
+                        %<element>Text <!--Comment--> More text</element>
+                        
+                        %put the text in different cells:
+                        % if (~iscell(ptext)) ptext = {ptext}; end
+                        % ptext{length(ptext)+1} = text;
+                        
+                        %just append the text
+                        ptext.(ptextflag) = [ptext.(ptextflag) text.(textflag)];
+                    end
+                end
+            end
+            
+        end
+    end
+end
+
+% ----- Subfunction getNodeData -----
+function [text,name,attr,childs,textflag] = getNodeData(theNode)
+    % Create structure of node info.
+    
+    %make sure name is allowed as structure name
+    name = toCharArray(getNodeName(theNode))';
+    name = strrep(name, '-', '_dash_');
+    name = strrep(name, ':', '_colon_');
+    name = strrep(name, '.', '_dot_');
+
+    attr = parseAttributes(theNode);
+    if (isempty(fieldnames(attr))) 
+        attr = []; 
+    end
+    
+    %parse child nodes
+    [childs,text,textflag] = parseChildNodes(theNode);
+    
+    if (isempty(fieldnames(childs)) && isempty(fieldnames(text)))
+        %get the data of any childless nodes
+        % faster than if any(strcmp(methods(theNode), 'getData'))
+        % no need to try-catch (?)
+        % faster than text = char(getData(theNode));
+        text.(textflag) = toCharArray(getTextContent(theNode))';
+    end
+    
+end
+
+% ----- Subfunction parseAttributes -----
+function attributes = parseAttributes(theNode)
+    % Create attributes structure.
+
+    attributes = struct;
+    if hasAttributes(theNode)
+       theAttributes = getAttributes(theNode);
+       numAttributes = getLength(theAttributes);
+
+       for count = 1:numAttributes
+            %attrib = item(theAttributes,count-1);
+            %attr_name = regexprep(char(getName(attrib)),'[-:.]','_');
+            %attributes.(attr_name) = char(getValue(attrib));
+
+            %Suggestion of Adrian Wanner
+            str = toCharArray(toString(item(theAttributes,count-1)))';
+            k = strfind(str,'='); 
+            attr_name = str(1:(k(1)-1));
+            attr_name = strrep(attr_name, '-', '_dash_');
+            attr_name = strrep(attr_name, ':', '_colon_');
+            attr_name = strrep(attr_name, '.', '_dot_');
+            attributes.(attr_name) = str((k(1)+2):(end-1));
+       end
+    end
+end
\ No newline at end of file

pinhole/back_project_line.m

@@ -11,8 +11,8 @@ function [e_s, x0] = back_project_line(camA, camB, lineA, lineB)
 	% Input:
 	%	camA		calibration structure, camera A
 	%	camB		calibration structure, camera B
-	%	lineA		image coordinates of line in camera A [x0 y0; x1 y1]
-	%	lineB		image coordinates of line in camera B [x0 y0; x1 y1]
+	%	lineA		image coordinates of line in camera A [x0 x1; y0 y1]
+	%	lineB		image coordinates of line in camera B [x0 x1; y0 y1]
 	
 	%% correct for distortion
 	im_line_A		= inverse_radial_distortion(lineA, camA.ic, camA.kr);
@@ -34,6 +34,10 @@ function [e_s, x0] = back_project_line(camA, camB, lineA, lineB)
 	n_B				= n_B / norm(n_B);
 	d_B				= -dot(camB.xc, n_B);			% use known point on plane to get offset
 	
+	if abs(dot(n_B,n_A)) > 0.99
+		warning('planes are nearly parallel, results may be inaccurate');
+	end
+	
 	%% reconstruct line
 	e_s				= cross(n_A, n_B);
 	e_s				= e_s / norm(e_s);

pinhole/dlt34.m

-function P = dlt34(xk, yk)
+function P = dlt34(xkt, ykt)
 	% solve the set of similarity relations
 	% xk ~ Pyk
 	% 
@@ -6,55 +6,49 @@ function P = dlt34(xk, yk)
 	%     yk = [y1k y2k y3k y4k]' is a 4x1 vector
 	% 
 	% to find the 3x4 matrix P
-	
-	% set up the problem as solving
-	% xk' * H * P * yk = 0
-	%
-	% leveraging xk'*H*xk = 0
-	% however, for xk on R^3, the dimension of H_m is 3
-	% i.e. there are three basis-matrices H_m that can satisfy xk'*H*xk = 0
-	% these are:
-	% 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]
 	%
-	% so we get three sets of equations
-	% bk' * p = [0 0 0]'
+	% references
+	% [1] Hartley & Zisserman (2003) pp 179-181
+	
+	%% number of similarity relations we have
+	N			= size(xkt, 2);
+	assert(size(ykt,2) == N);
+	assert(size(xkt,1) == 3);
+	assert(size(ykt,1) == 4);
 	
-	% number of similarity relations we have
-	N			= size(xk, 2);
-	assert(size(yk,2) == N);
-	assert(size(xk,1) == 3);
-	assert(size(yk,1) == 4);
+	%% precondition the problem
+	% rescale xk~ and yk~ so that they have zero mean and std sqrt(2) and
+	% sqrt(3) respectively
+	x0			= mean(xkt, 2); % mean image space position
+	y0			= mean(ykt, 2);	% mean object space position
+	x_var		= var(xkt, [], 2);
+	y_var		= var(ykt, [], 2);
+	sx			= sqrt(1/2 ./ x_var);
+	sy			= sqrt(1/3 ./ y_var);
+	T			= diag([sx(1:2); 1]) - [zeros(3,2), [sx(1:2).*x0(1:2);0]];
+	U			= diag([sy(1:3); 1]) - [zeros(4,3), [sy(1:3).*y0(1:3);0]];
+	xk			= T * xkt;
+	yk			= U * ykt;
 	
-	% set up matrix B
-	% with rows b_k
-	% that satisfy b_k * p = 0
-	B			= zeros(N*3, 12);
-	for k = 1 : N
-		x					= xk(:, k);
-		y					= yk(:, k);
-		B(3*(k-1)+1, :)= [ 0; +x(3)*y(1); -x(2)*y(1); 0; +x(3)*y(2); -x(2)*y(2); 0; +x(3)*y(3); -x(2)*y(3); 0; +x(3)*y(4); -x(2)*y(4)];
-		B(3*(k-1)+2, :)= [ -x(3)*y(1); 0; +x(1)*y(1); -x(3)*y(2); 0; +x(1)*y(2); -x(3)*y(3); 0; +x(1)*y(3); -x(3)*y(4); 0; +x(1)*y(4)];
-		B(3*(k-1)+3, :)= [ +x(2)*y(1); -x(1)*y(1); 0; +x(2)*y(2); -x(1)*y(2); 0; +x(2)*y(3); -x(1)*y(3); 0; +x(2)*y(4); -x(1)*y(4); 0];
+	%% solve preconditioned problem
+	% write equation (7.2) for each point, which corresponds to 2*N
+	% constraints on P written as
+	% A * [P(1,:)' P(2,:)' P(3,:)'] = 0
+	A			= zeros(2*N, 12);
+	for i = 1:N
+		A(2*i-1, :) = [0;0;0;0; -xk(3,i)*yk(:,i); xk(2,i)*yk(:,i)];
+		A(2*i  , :) = [xk(3,i)*yk(:,i); 0;0;0;0; -xk(1,i)*yk(:,i)];
 	end
+	% solve the equation Ap = 0 in the least squares sense by finding the
+	% minimum singular value of A and its associated vector
+	[~,D,V]		= svd(A);
+	[~,imin]	= min(diag(D));
+	p			= V(:,imin);
+	% reshape the flatted p back to full size P
+	P			= reshape(p, [4 3])';
 	
-	% now our solution p is the solution of
-	% B*p = 0
-	% i.e. p is in the null space of B
-	% can be solved with SVD
-	% 
-	% we add the additional constraint that |p| = 1
-	% to find the minimal P
-	%
-	% p is the unit singular vector of A corresponding to the last column of
-	% V, when the elements in S are ranked in decreasing size
-	% which, helpfully, matlab does by default
-	[Q,D]		= eig(B.' * B);
-	D			= diag(D);
-	[~,idx]	= min(abs(D));
-	p			= Q(:, idx);
-	P			= reshape(p, [3 4]);
+	%% transform back preconditioning
+	P			= T^-1 * P * U;
 end
 	
 		

pinhole/pinhole_fit_3d.m

@@ -21,27 +21,29 @@ function [new_model, ij_res] = pinhole_fit_3d(ij, uvw, opts)
 	% which minimises disparity between model and data
 	% 
 	
-	%% preliminary fit
-	% solve using direct linear transform
-	N			= size(uvw, 2); 
+	%% size of problem
+	N						= size(uvw, 2); 
 	assert(size(ij ,2) == N);
 	assert(size(uvw,1) == 3);
 	assert(size(ij ,1) == 2);
+	x_tilde					= [uvw; ones(1,N)];		% object space, homogeneous coords
+	y_tilde					= [ij; ones(1,N)];		% image space, homogeneous coords
 	
-	uvw1					= [uvw; ones(1,N)];
-	P						= dlt34([ij; ones(1,N)], uvw1);
+	%% preliminary fit
+	% solve using direct linear transform
+	P						= dlt34(y_tilde, x_tilde);
 	
 	%% construct options structure for fitting
 	% get approximate diagonal of sensor
 	% used in normalisation stage
-	diag_px				= norm(max(ij,[],2) - min(ij,[],2));
+	diag_px					= norm(max(ij,[],2) - min(ij,[],2));
 	if nargin < 3
-		opts			= struct(	'square', false, ...
+		opts				= struct(	'square', false, ...
 									'skew', true, ...
 									'distortion', true, ...
 									'diag', diag_px);
 	else
-		opts.diag		= diag_px;
+		opts.diag			= diag_px;
 	end
 	
 	%% decompose pinhole model
@@ -68,7 +70,7 @@ function [new_model, ij_res] = pinhole_fit_3d(ij, uvw, opts)
 	
 	%% constrained linear-least squares to solve G 
 	% with square pixel and or zero skewness constraints
-	Xp						= R*[eye(3),-xc(:)] * uvw1;
+	Xp						= R*[eye(3),-xc(:)] * x_tilde;
 	lambda					= Xp(3,:);
 	% markers in physical coordinates on image plane
 	xp						= Xp(1,:)./lambda;	
@@ -108,9 +110,9 @@ function [new_model, ij_res] = pinhole_fit_3d(ij, uvw, opts)
 	%% estimate radial distortion
 	% we fit a radial distortion model to the residuals of the pinhole model
 	% fit
-	lambda				=  P(3,:) * uvw1;
-	ij_model			= [P(1,:) * uvw1 ./ lambda;
-						   P(2,:) * uvw1 ./ lambda];
+	lambda				=  P(3,:) * x_tilde;
+	ij_model			= [P(1,:) * x_tilde ./ lambda;
+						   P(2,:) * x_tilde ./ lambda];
 	X0					= [mean(ij(1,:)) mean(ij(2,:)) 0 0];
 	
 	fitopts				= optimset('Display', 'off', 'TolX', 1E-9);