Rev 238 | Rev 240 | Go to most recent revision | Blame | Compare with Previous | Last modification | View Log | RSS feed
function MSE = alignSubScansMarkers(calibrationFileName, alnFileName)
%ALIGNSUBSCANSMARKERS Determines an exact alignment of sub scans (scans
% from e.g. one revolution of the rotation stage).
% The method searches for circular white markers of a specific diameter.
% White frames corresponding to each sub scan must be available.
% A coarse alignment in the form of an aln-file must be provided.
%
% 2017 Jakob Wilm, DTU
initialAlign = readMeshLabALN(alnFileName);
[alnFilePath, ~, ~] = fileparts(alnFileName);
calibration = readOpenCVXML(calibrationFileName);
% correct for Matlab 1-indexing in principle point coordinates
calibration.K0(1:2, 3) = calibration.K0(1:2, 3)+1;
calibration.K1(1:2, 3) = calibration.K1(1:2, 3)+1;
% full projection matrices in Matlab convention
P0 = transpose(calibration.K0*[eye(3) zeros(3,1)]);
P1 = transpose(calibration.K1*[calibration.R1 calibration.T1']);
% matlab cam params for undistortion
camParams0 = cameraParameters('IntrinsicMatrix', calibration.K0', 'RadialDistortion', calibration.k0([1 2 5]), 'TangentialDistortion', calibration.k0([3 4]));
camParams1 = cameraParameters('IntrinsicMatrix', calibration.K1', 'RadialDistortion', calibration.k1([1 2 5]), 'TangentialDistortion', calibration.k1([3 4]));
% matlab struct for triangulation
camStereoParams = stereoParameters(camParams0, camParams1, calibration.R1', calibration.T1');
nSubScans = length(initialAlign);
% 3D coordinates of markers in local camera frame
E = cell(nSubScans, 1);
% 3D coordinates of markers in global initial alignment
Eg = cell(size(E));
% find 3D markers coordinates
for i=1:nSubScans
%for i=5:5
% load point cloud
pcFileName = fullfile(alnFilePath, initialAlign(i).FileName);
pcFilePath = fileparts(pcFileName);
pc = pcread(pcFileName);
Q = pc.Location;
idString = strsplit(initialAlign(i).FileName, {'.ply', '_'});
idString = idString{end-1};
% load white frames
frame0 = imread(fullfile(pcFilePath, ['sequence_' idString], 'frames0_0.png'));
frame1 = imread(fullfile(pcFilePath, ['sequence_' idString], 'frames1_0.png'));
e0Coords = autoDetectMarkers(frame0);
e1Coords = autoDetectMarkers(frame1);
%e0Coords = manuallyDetectMarkers(frame0);
%e1Coords = manuallyDetectMarkers(frame1);
%[e0Coords, conf0] = detectMarkersSubpix(frame0, e0Coords, P0, Q);
%[e1Coords, conf1] = detectMarkersSubpix(frame1, e1Coords, P1, Q);
if(length(e0Coords) < 1 || length(e1Coords) < 1)
continue;
end
% figure;
% subplot(1,2,1);
% imshow(frame0);
% hold('on');
% plot(e0Coords(:,1), e0Coords(:,2), 'rx', 'MarkerSize', 15);
% subplot(1,2,2);
% imshow(frame1);
% hold('on');
% plot(e1Coords(:,1), e1Coords(:,2), 'rx', 'MarkerSize', 15);
% drawnow;
e0Coords = undistortPointsFast(e0Coords, camParams0);
e1Coords = undistortPointsFast(e1Coords, camParams1);
% match ellipse candidates between cameras based on projection
E0 = projectOntoPointCloud(e0Coords, P0, Q);
E1 = projectOntoPointCloud(e1Coords, P1, Q);
matchedPairs = nan(size(E0, 1), 2);
nMatchedPairs = 0;
for j=1:size(E0, 1)
% should use pdist2 instead..
sqDists = sum((E1 - repmat(E0(j,:), size(E1, 1), 1)).^2, 2);
[minSqDist, minSqDistIdx] = min(sqDists);
if(minSqDist < 1^2)
nMatchedPairs = nMatchedPairs + 1;
matchedPairs(nMatchedPairs, :) = [j, minSqDistIdx];
end
end
matchedPairs = matchedPairs(1:nMatchedPairs, :);
figure;
subplot(1,2,1);
imshow(frame0);
hold('on');
plot(e0Coords(matchedPairs(:, 1),1), e0Coords(matchedPairs(:, 1),2), 'rx', 'MarkerSize', 15);
subplot(1,2,2);
imshow(frame1);
hold('on');
plot(e1Coords(matchedPairs(:, 2),1), e1Coords(matchedPairs(:, 2),2), 'rx', 'MarkerSize', 15);
drawnow;
% % triangulate marker centers (lens correction has been performed)
% [E{i}, e] = triangulate(e0Coords(matchedPairs(:, 1),:), e1Coords(matchedPairs(:, 2),:), camStereoParams);
% E{i} = E{i}(e<3.0, :);
% display(e);
[E{i}, e] = detectMarkersStereoSubpix(frame0, frame1, E0(matchedPairs(:, 1), :), camStereoParams, pc);
display(e);
% write point cloud with marker (debugging)
pcDebug = pointCloud([pc.Location; E{i}], 'Color', [pc.Color; repmat([255, 0, 0], size(E{i}, 1), 1)]);
pcwrite(pcDebug, 'pcDebug.ply');
% bring markers into initial alignment
[U,~,V] = svd(initialAlign(i).Rotation);
Ri = U*V';
Ti = initialAlign(i).Translation;
Eg{i} = E{i}*Ri' + repmat(Ti', size(E{i}, 1), 1);
end
% show found markers in initial alignment
figure;
hold('on');
for i=1:nSubScans
% fix Ri to be orthogonal
[U,~,V] = svd(initialAlign(i).Rotation);
Ri = U*V';
% bring point cloud into initial alignment
pcFileName = fullfile(alnFilePath, initialAlign(i).FileName);
pc = pcread(pcFileName);
tform = affine3d([Ri' [0;0;0]; initialAlign(i).Translation' 1]);
pcg = pctransform(pc, tform);
pcshow(pcg);
xlabel('x');
ylabel('y');
zlabel('z');
plot3(Eg{i}(:,1), Eg{i}(:,2), Eg{i}(:,3), '.', 'MarkerSize', 15);
title('Initial Alignment');
end
% match markers between poses using initial alignment
Pg = {};
P = {};
for i=1:nSubScans
for j=1:size(Eg{i}, 1)
pg = Eg{i}(j,:);
p = E{i}(j,:);
matched = false;
for k=1:size(Pg, 2)
clusterCenter = mean(cat(1, Pg{:,k}), 1);
if(sum((pg - clusterCenter).^2) < 3^2)
% store in global frame
Pg{i,k} = pg;
% store in local frame
P{i,k} = p;
matched = true;
break;
end
end
% create new cluster
if(not(matched))
Pg{i,end+1} = pg;
P{i,end+1} = p;
end
end
end
% run optimization
alignment = groupwiseOrthogonalProcrustes(P, initialAlign);
% show found markers in optimized alignment
figure;
hold('on');
for i=1:nSubScans
% fix Ri to be orthogonal
[U,~,V] = svd(alignment(i).Rotation);
Ri = U*V';
Ti = alignment(i).Translation;
Ea = E{i}*Ri' + repmat(Ti', size(E{i}, 1), 1);
% bring point cloud into optimized alignment
pc = pcread(initialAlign(i).FileName);
tform = affine3d([Ri' [0;0;0]; initialAlign(i).Translation' 1]);
pcg = pctransform(pc, tform);
pcshow(pcg);
xlabel('x');
ylabel('y');
zlabel('z');
plot3(Ea(:,1), Ea(:,2), Ea(:,3), '.', 'MarkerSize', 15);
title('Optimized Alignment');
end
% write to ALN file
for i=1:length(alignment)
alignment(i).FileName = initialAlign(i).FileName;
end
writeMeshLabALN(alignment, strrep(alnFileName, '.aln', 'Optimized.aln'));
end
function e = autoDetectMarkers(frame, P, pointCloud)
% create mask based on morphology
g = rgb2gray(frame);
% g(g>254) = 0;
% bw = imbinarize(g, 'adaptive', 'Sensitivity', 10^(-50));
bw = imbinarize(g, 0.10);
cc = bwconncomp(bw);
rp = regionprops(cc, 'Area', 'Solidity', 'Eccentricity', 'Centroid');
idx = ([rp.Area] > 100 & [rp.Area] < 1000 & [rp.Solidity] > 0.9);
e = cat(1, rp(idx).Centroid);
end
function e = manuallyDetectMarkers(frame, P, pointCloud)
e = [];
%edges = edge(rgb2gray(frame), 'Canny', [0.08 0.1], 2);
figure;
hold('on');
imshow(frame);
title('Close figure to end.');
set(gcf, 'pointer', 'crosshair');
set(gcf, 'WindowButtonDownFcn', @clickCallback);
uiwait;
function clickCallback(caller, ~)
p = get(gca, 'CurrentPoint');
p = p(1, 1:2);
e = [e; p(:, 1:2)];
if(not(isempty(el)))
figure(caller);
hold('on');
plot(el(1), el(2), 'rx', 'MarkerSize', 15);
end
end
end
function [e, conf] = detectMarkersSubpix(frame, initGuesses, P, Q)
% Detect a marker in a single frame by rectifying to the image and
% performing image registration.
% create mask based on morphology
g = rgb2gray(frame);
g(g>254) = 0;
bw = imbinarize(g);
cc = bwconncomp(bw);
labels = labelmatrix(cc);
% project point cloud into image
q = [Q ones(size(Q,1),1)]*P;
q = q./[q(:,3) q(:,3) q(:,3)];
e = zeros(size(initGuesses));
conf = zeros(size(initGuesses, 1), 1);
nMarkersFound = 0;
for i=1:size(initGuesses, 1)
labelId = labels(round(initGuesses(i,2)), round(initGuesses(i,1)));
labelMask = (labels == labelId);
labelMask = imdilate(labelMask, strel('disk', 3, 0));
if(sum(sum(labelMask)) < 10 || sum(sum(labelMask)) > 1000)
continue;
end
% determine 3D points that are part of the marker
% note: we should probably undistort labelMask
pointMask = false(size(q, 1), 1);
for j=1:size(q,1)
if(round(q(j,2)) > size(labelMask, 1) || round(q(j,1)) > size(labelMask, 2) || round(q(j,2)) < 1 || round(q(j,1)) < 1)
continue;
end
if(labelMask(round(q(j,2)), round(q(j,1))))
pointMask(j) = true;
end
end
if(sum(pointMask)) < 10
continue;
end
% project 3D points onto local plane
[~, sc, ~] = pca(Q(pointMask, :));
Qlocal = sc(:, 1:2);
% synthetic marker in high res. space
m = zeros(151, 151);
[x, y] = meshgrid(1:151, 1:151);
m((x(:)-76).^2 + (y(:)-76).^2 <= 50^2) = 1.0;
% relation between marker space (px) and true marker/local plane(mm)
% true marker diameter is 1.75mm
mScale = 101/1.4; %px/mm
mShift = 76; %px
% build homography from image to marker space
H = fitgeotrans(q(pointMask, 1:2), mScale*Qlocal+mShift, 'projective');
%Hdlt = Hest_DLT([mScale*Qlocal+mShift, ones(size(Qlocal, 1), 1)]', q(pointMask,:)');
%H = projective2d(Hdlt');
% bring image of marker into marker space
imMarkerSpace = imwarp(frame, H, 'OutputView', imref2d(size(m)));
imMarkerSpace = rgb2gray(im2double(imMarkerSpace));
%figure; imshowpair(imMarkerSpace, m);
% perform image registration
% might be better off using subpixel image correlation
[opt, met] = imregconfig('multimodal');
T = imregtform(m, imMarkerSpace, 'translation', opt, met, 'DisplayOptimization', false);
rege = imwarp(m, T, 'OutputView', imref2d(size(m)));
%figure; imshowpair(imMarkerSpace, rege);
% measure of correlation
confI = sum(sum(imMarkerSpace .* rege))/sqrt(sum(sum(imMarkerSpace) * sum(sum(rege))));
%confI = 1.0;
if confI<0.4
continue;
end
fprintf('Found marker with confidence: %f\n', confI);
% transform marker space coordinates (76,76) to frame space
el = T.transformPointsForward([76, 76]);
el = H.transformPointsInverse(el);
nMarkersFound = nMarkersFound+1;
e(nMarkersFound,:) = el;
conf(nMarkersFound) = confI;
end
e = e(1:nMarkersFound, :);
conf = conf(1:nMarkersFound);
end
function [E, conf] = detectMarkersStereoSubpix(frame0, frame1, initGuesses, camStereoParams, pc)
% Detect a marker in stereo frame set by minimizing the difference to
% projected images of 3d marker
normals = pcnormals(pc, 6);
frame0 = rgb2gray(frame0);
frame1 = rgb2gray(frame1);
nMarkers = size(initGuesses, 2);
E = zeros(nMarkers, 3);
conf = zeros(nMarkers, 1);
for i=1:nMarkers
pI = initGuesses(i,:);
e0 = camStereoParams.CameraParameters1.worldToImage(eye(3,3), zeros(3,1), pI);
e1 = camStereoParams.CameraParameters2.worldToImage(camStereoParams.RotationOfCamera2, camStereoParams.TranslationOfCamera2, pI);
% center of support region
e0Center = round(e0);
e1Center = round(e1);
% find initial normal
idx = pc.findNearestNeighbors(pI, 1);
nI = double(normals(idx, :));
margin = 25;
[x,y] = meshgrid(e0Center(1)-margin:e0Center(1)+margin, e0Center(2)-margin:e0Center(2)+margin);
e0ImCoords = [x(:), y(:)];
[x,y] = meshgrid(e1Center(1)-margin:e1Center(1)+margin, e1Center(2)-margin:e1Center(2)+margin);
e1ImCoords = [x(:), y(:)];
e0UndistImCoords = undistortPointsFast(e0ImCoords, camStereoParams.CameraParameters1);
e0NormImCoords = camStereoParams.CameraParameters1.pointsToWorld(eye(3,3), [0, 0, 1], e0UndistImCoords);
e1UndistImCoords = undistortPointsFast(e1ImCoords, camStereoParams.CameraParameters2);
e1NormImCoords = camStereoParams.CameraParameters2.pointsToWorld(eye(3,3), [0, 0, 1], e1UndistImCoords);
x0 = [pI nI(1:2)/nI(3) 70.0 70.0];
options = optimset('Algorithm', 'levenberg-marquardt', 'Display', 'iter-detailed', 'OutputFcn', @out, 'MaxIter', 30, 'TolFun', 10^(-5), 'TolX', 0);
[x, conf(i), ~] = lsqnonlin(@circleResiduals, x0, [], [], options);
display(x);
E(i,:) = x(1:3);
end
function stop = out(x, optimValues, state)
% r = optimValues.residual;
%
% figure;
% subplot(1,2,1);
% imagesc(reshape(r(1:length(e0NormImCoords)), 2*margin+1, 2*margin+1), [-50 50]);
% subplot(1,2,2);
% imagesc(reshape(r(length(e0NormImCoords)+1:end), 2*margin+1, 2*margin+1), [-50 50]);
% drawnow;
%
% display(x);
stop = false;
end
function r = circleResiduals(x)
p0 = x(1:3);
p1 = x(1:3) * camStereoParams.RotationOfCamera2 + camStereoParams.TranslationOfCamera2;
n0 = [x(4:5) 1.0];
n0 = 1.0/norm(n0) * n0;
n1 = n0 * camStereoParams.RotationOfCamera2;
r = zeros(length(e0NormImCoords) + length(e1NormImCoords), 1);
% norminal radius of markers
markerRadius = 1.4/2.0; %mm
% half-width of ramp
w = 0.3; %mm
p0n0 = (p0*n0');
p1n1 = (p1*n1');
for k=1:length(e0NormImCoords)
% dermine homogenous coordinate on the hypothesized plane
t = p0n0/([e0NormImCoords(k,:), 1]*n0');
d = norm(p0 - t*[e0NormImCoords(k,:), 1.0]);
imVal = double(frame0(e0ImCoords(k,2), e0ImCoords(k,1)));
% "saturated" ramp function for marker disc shape
weight = max(min(1.0, -1.0/(2*w)*(d-markerRadius)+0.5), 0.0);
r(k) = x(6)*weight - imVal;
end
for k=1:length(e1NormImCoords)
% dermine z coordinate on the hypothesized plane
t = p1n1/([e1NormImCoords(k,:), 1]*n1');
d = norm(p1 - t*[e1NormImCoords(k,:), 1.0]);
imVal = double(frame1(e1ImCoords(k,2), e1ImCoords(k,1)));
% "saturated" ramp function for marker disc shape
weight = max(min(1.0, -1.0/(2*w)*(d-markerRadius)+0.5), 0.0);
r(length(e0NormImCoords)+k) = x(7)*weight - imVal;
end
% figure;
% subplot(1,2,1);
% imagesc(reshape(r(1:length(e0NormImCoords)), 2*margin+1, 2*margin+1), [-50 50]);
% subplot(1,2,2);
% imagesc(reshape(r(length(e0NormImCoords)+1:end), 2*margin+1, 2*margin+1), [-50 50]);
% drawnow;
end
end
function E = projectOntoPointCloud(e, P, pointCloud)
% Project 2d point coordinates onto pointCloud to find corresponding 3d
% point coordinates.
q = [pointCloud ones(size(pointCloud,1),1)]*P;
q = q(:,1:2)./[q(:,3) q(:,3)];
E = nan(size(e,1), 3);
for i=1:size(e, 1)
sqDists = sum((q - repmat(e(i,:), size(q, 1), 1)).^2, 2);
[sqDistsSorted, sortIdx] = sort(sqDists);
neighbors = (sqDistsSorted < 4.0^2);
distsSorted = sqrt(sqDistsSorted(neighbors));
invDistsSorted = 1.0/distsSorted;
sortIdx = sortIdx(neighbors);
nNeighbors = sum(neighbors);
if(nNeighbors >= 2)
E(i, :) = 0;
for j=1:nNeighbors
E(i, :) = E(i, :) + invDistsSorted(j)/sum(invDistsSorted) * pointCloud(sortIdx(j), :);
end
end
end
end
function H = Hest_DLT(q1, q2)
% Estimate the homography between a set of point correspondences using the
% direct linear transform algorithm.
%
% Input:
% q1: 3xN matrix of homogenous point coordinates from camera 1.
% q2: 3xN matrix of corresponding points from camera 2.
% Output:
% H: 3x3 matrix. The Fundamental Matrix estimate.
%
% Note that N must be at least 4.
% See derivation in Aanaes, Lecture Notes on Computer Vision, 2011
% Normalize points
[T1,invT1] = normalizationMat(q1);
q1_tilde = T1*q1;
T2 = normalizationMat(q2);
q2_tilde = T2*q2;
% DLT estimation
N = size(q1_tilde,2);
assert(size(q2_tilde,2)==N);
B = zeros(3*N,9);
for i=1:N
q1i = q1_tilde(:,i);
q2i = q2_tilde(:,i);
q1_x = [0 -q1i(3) q1i(2); q1i(3) 0 -q1i(1); -q1i(2) q1i(1) 0];
biT = kron(q2i', q1_x);
B(3*(i-1)+1:3*i, :) = biT;
end
[U,S,~] = svd(B');
[~,idx] = min(diag(S));
h = U(:,idx);
H_tilde = reshape(h, 3, 3);
% Unnormalize H
H = invT1*H_tilde*T2;
% Arbitrarily chose scale
H = H * 1/H(3,3);
end
function [T,invT] = normalizationMat(q)
% Gives a normalization matrix for homogeneous coordinates
% such that T*q will have zero mean and unit variance.
% See Aanaes, Computer Vision Lecture Notes 2.8.2
%
% q: (M+1)xN matrix of N MD points in homogenous coordinates
%
% Extended to also efficiently compute the inverse matrix
% DTU, 2013, Jakob Wilm
[M,N] = size(q);
M = M-1;
mu = mean(q(1:M,:),2);
q_bar = q(1:M,:)-repmat(mu,1,N);
s = mean(sqrt(diag(q_bar'*q_bar)))/sqrt(2);
T = [eye(M)/s, -mu/s; zeros(1,M) 1];
invT = [eye(M)*s, mu; zeros(1,M) 1];
end
Generated by GNU Enscript 1.6.6.