Commit 5c9b410c authored by jml1g18's avatar jml1g18
Browse files

update of pinhole fitting procedure and some auxiliary functions

parent 60fa49e4
function parsave(fname, s, varargin)
save(fname, '-struct', 's', varargin{:});
end
\ No newline at end of file
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
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
......@@ -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);
......
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
......@@ -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);
......
Supports Markdown
0% or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment