WebSVN – seema-scanner – Diff – /matlab/alignSubScansMarkers.m



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);
 
 
 
 
calibration = readOpenCVXML(calibrationFileName);
 
% full projection matrices in Matlab convention
P0 = transpose(calibration.K0*[eye(3) zeros(3,1)]);

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

 
% find 3D markers coordinates 
for i=1:nSubScans
 
    % load point cloud
    pc = pcread(initialAlign(i).FileName);
    Q = pc.Location;
    idString = strsplit(initialAlign(i).FileName, {'.ply', '_'});
    idString = idString{end-1};
    
    % load white frames
    scanDir = strsplit(initialAlign(i).FileName, '/');
    scanDir = fullfile(scanDir{1:end-1});
    frame0 = imread(fullfile(scanDir, ['sequence_' idString], 'frames0_0.png'));
    frame1 = imread(fullfile(scanDir, ['sequence_' idString], 'frames1_0.png'));
 
    %e0Coords = autoDetectMarkers(frame0, P0, Q);
    %e1Coords = autoDetectMarkers(frame1, P1, Q);
    
    e0Coords = manuallyDetectMarkers(frame0, P0, Q);
    e1Coords = manuallyDetectMarkers(frame1, P1, Q);
    
    e0Coords = undistortPoints(e0Coords, camParams0);
    e1Coords = undistortPoints(e1Coords, camParams1);
 
    % match ellipse candidates between cameras based on projection

        end
    end
    matchedPairs = matchedPairs(1:nMatchedPairs, :);
    
    % triangulate marker centers (lens correction has been performed)
    [E{i}, e] = triangulate(e0Coords(matchedPairs(:, 1),:), e1Coords(matchedPairs(:, 2),:), camStereoParams);
    E{i} = E{i}(e<0.5, :);
    
    % bring into initial alignment
    [U,~,V] = svd(initialAlign(i).Rotation);
    Ri = U*V';
    Ti = initialAlign(i).Translation;

% 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)
 

            if(labelMask(round(q(j,2)), round(q(j,1))))
                pointMask(j) = true;
            end
        end
        
        if(sum(pointMask)) < 50
            continue;
        end
        
        % project 3D points onto local plane
        [~, sc, ~] = pca(Q(pointMask, :));

        [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.8; %px/mm
        mShift = 76; %px
        
        % build homography from image to marker space
        H = fitgeotrans(q(pointMask, 1:2), mScale*Qlocal+mShift,  'projective');
        

        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', true);
        
        rege = imwarp(m, T, 'OutputView', imref2d(size(m)));
        %figure; imshowpair(imMarkerSpace, rege);
        
        % measure of correlation

Rev 213	Rev 214
Line 1...	Line 1...
1	`function MSE = alignSubScansMarkers(calibrationFileName, scanDir, alnFileName)`	1	`function MSE = alignSubScansMarkers(calibrationFileName, alnFileName)`
2	`%ALIGNSUBSCANSMARKERS Determines an exact alignment of sub scans (scans`	2	`%ALIGNSUBSCANSMARKERS Determines an exact alignment of sub scans (scans`
3	`% from e.g. one revolution of the rotation stage).`	3	`% from e.g. one revolution of the rotation stage).`
4	`% The method searches for circular white markers of a specific diameter.`	4	`% The method searches for circular white markers of a specific diameter.`
5	`% White frames corresponding to each sub scan must be available.`	5	`% White frames corresponding to each sub scan must be available.`
6	`% A coarse alignment in the form of an aln-file must be provided.`	6	`% A coarse alignment in the form of an aln-file must be provided.`
7	`%`	7	`%`
8	`% 2017 Jakob Wilm, DTU`	8	`% 2017 Jakob Wilm, DTU`
9		9
10	`initialAlign = readMeshLabALN(alnFileName);`	10	`initialAlign = readMeshLabALN(alnFileName);`
11	`whiteFrameDirs = dir(fullfile(scanDir, 'sequence_*'));`	-
12		-
13	`assert(length(whiteFrameDirs) == length(initialAlign));`	-
14		11
15	`calibration = readOpenCVXML(calibrationFileName);`	12	`calibration = readOpenCVXML(calibrationFileName);`
16		13
17	`% full projection matrices in Matlab convention`	14	`% full projection matrices in Matlab convention`
18	`P0 = transpose(calibration.K0*[eye(3) zeros(3,1)]);`	15	`P0 = transpose(calibration.K0*[eye(3) zeros(3,1)]);`
Line 23...	Line 20...
23	`camParams1 = cameraParameters('IntrinsicMatrix', calibration.K1', 'RadialDistortion', calibration.k1([1 2 5]), 'TangentialDistortion', calibration.k1([3 4]));`	20	`camParams1 = cameraParameters('IntrinsicMatrix', calibration.K1', 'RadialDistortion', calibration.k1([1 2 5]), 'TangentialDistortion', calibration.k1([3 4]));`
24		21
25	`% matlab struct for triangulation`	22	`% matlab struct for triangulation`
26	`camStereoParams = stereoParameters(camParams0, camParams1, calibration.R1', calibration.T1');`	23	`camStereoParams = stereoParameters(camParams0, camParams1, calibration.R1', calibration.T1');`
27		24
28	`nSubScans = length(whiteFrameDirs);`	25	`nSubScans = length(initialAlign);`
29		26
30	`% 3D coordinates of markers in local camera frame`	27	`% 3D coordinates of markers in local camera frame`
31	`E = cell(nSubScans, 1);`	28	`E = cell(nSubScans, 1);`
32		29
33	`% 3D coordinates of markers in global initial alignment`	30	`% 3D coordinates of markers in global initial alignment`
Line 35...	Line 32...
35		32
36	`% find 3D markers coordinates`	33	`% find 3D markers coordinates`
37	`for i=1:nSubScans`	34	`for i=1:nSubScans`
38		35
39	`% load point cloud`	36	`% load point cloud`
40	`pc = pcread(fullfile(scanDir, initialAlign(i).FileName));`	37	`pc = pcread(initialAlign(i).FileName);`
41	`Q = pc.Location;`	38	`Q = pc.Location;`
42	`idString = initialAlign(i).FileName(12:end-4);`	39	`idString = strsplit(initialAlign(i).FileName, {'.ply', '_'});`
-		40	`idString = idString{end-1};`
43		41
44	`% load white frames`	42	`% load white frames`
-		43	`scanDir = strsplit(initialAlign(i).FileName, '/');`
-		44	`scanDir = fullfile(scanDir{1:end-1});`
45	`frame0 = imread(fullfile(scanDir, ['sequence_' idString], 'frames0_0.png'));`	45	`frame0 = imread(fullfile(scanDir, ['sequence_' idString], 'frames0_0.png'));`
46	`frame1 = imread(fullfile(scanDir, ['sequence_' idString], 'frames1_0.png'));`	46	`frame1 = imread(fullfile(scanDir, ['sequence_' idString], 'frames1_0.png'));`
47		47
48	`e0Coords = autoDetectMarkers(frame0, P0, Q);`	48	`%e0Coords = autoDetectMarkers(frame0, P0, Q);`
49	`e1Coords = autoDetectMarkers(frame1, P1, Q);`	49	`%e1Coords = autoDetectMarkers(frame1, P1, Q);`
50		50
51	`%e0Coords = manuallyDetectMarkers(frame0, P0, Q);`	51	`e0Coords = manuallyDetectMarkers(frame0, P0, Q);`
52	`%e1Coords = manuallyDetectMarkers(frame1, P1, Q);`	52	`e1Coords = manuallyDetectMarkers(frame1, P1, Q);`
53		53
54	`e0Coords = undistortPoints(e0Coords, camParams0);`	54	`e0Coords = undistortPoints(e0Coords, camParams0);`
55	`e1Coords = undistortPoints(e1Coords, camParams1);`	55	`e1Coords = undistortPoints(e1Coords, camParams1);`
56		56
57	`% match ellipse candidates between cameras based on projection`	57	`% match ellipse candidates between cameras based on projection`
Line 73...	Line 73...
73	`end`	73	`end`
74	`end`	74	`end`
75	`matchedPairs = matchedPairs(1:nMatchedPairs, :);`	75	`matchedPairs = matchedPairs(1:nMatchedPairs, :);`
76		76
77	`% triangulate marker centers (lens correction has been performed)`	77	`% triangulate marker centers (lens correction has been performed)`
78	`E{i} = triangulate(e0Coords(matchedPairs(:, 1),:), e1Coords(matchedPairs(:, 2),:), camStereoParams);`	78	`[E{i}, e] = triangulate(e0Coords(matchedPairs(:, 1),:), e1Coords(matchedPairs(:, 2),:), camStereoParams);`
-		79	`E{i} = E{i}(e<0.5, :);`
79		80
80	`% bring into initial alignment`	81	`% bring into initial alignment`
81	`[U,~,V] = svd(initialAlign(i).Rotation);`	82	`[U,~,V] = svd(initialAlign(i).Rotation);`
82	`Ri = U*V';`	83	`Ri = U*V';`
83	`Ti = initialAlign(i).Translation;`	84	`Ti = initialAlign(i).Translation;`
Line 163...	Line 164...
163	`% write to ALN file`	164	`% write to ALN file`
164	`for i=1:length(alignment)`	165	`for i=1:length(alignment)`
165	`alignment(i).FileName = initialAlign(i).FileName;`	166	`alignment(i).FileName = initialAlign(i).FileName;`
166	`end`	167	`end`
167		168
168	`writeMeshLabALN(alignment, strrep(alnFileName, '.aln', '_opt.aln'));`	169	`writeMeshLabALN(alignment, strrep(alnFileName, '.aln', 'Optimized.aln'));`
169		170
170	`end`	171	`end`
171		172
172	`function e = autoDetectMarkers(frame, P, pointCloud)`	173	`function e = autoDetectMarkers(frame, P, pointCloud)`
173		174
Line 252...	Line 253...
252	`if(labelMask(round(q(j,2)), round(q(j,1))))`	253	`if(labelMask(round(q(j,2)), round(q(j,1))))`
253	`pointMask(j) = true;`	254	`pointMask(j) = true;`
254	`end`	255	`end`
255	`end`	256	`end`
256		257
257	`if(sum(pointMask)) < 100`	258	`if(sum(pointMask)) < 50`
258	`continue;`	259	`continue;`
259	`end`	260	`end`
260		261
261	`% project 3D points onto local plane`	262	`% project 3D points onto local plane`
262	`[~, sc, ~] = pca(Q(pointMask, :));`	263	`[~, sc, ~] = pca(Q(pointMask, :));`
Line 267...	Line 268...
267	`[x, y] = meshgrid(1:151, 1:151);`	268	`[x, y] = meshgrid(1:151, 1:151);`
268	`m((x(:)-76).^2 + (y(:)-76).^2 <= 50^2) = 1.0;`	269	`m((x(:)-76).^2 + (y(:)-76).^2 <= 50^2) = 1.0;`
269		270
270	`% relation between marker space (px) and true marker/local plane(mm)`	271	`% relation between marker space (px) and true marker/local plane(mm)`
271	`% true marker diameter is 1.75mm`	272	`% true marker diameter is 1.75mm`
272	`mScale = 101/1.75; %px/mm`	273	`mScale = 101/1.8; %px/mm`
273	`mShift = 76; %px`	274	`mShift = 76; %px`
274		275
275	`% build homography from image to marker space`	276	`% build homography from image to marker space`
276	`H = fitgeotrans(q(pointMask, 1:2), mScale*Qlocal+mShift, 'projective');`	277	`H = fitgeotrans(q(pointMask, 1:2), mScale*Qlocal+mShift, 'projective');`
277		278
Line 280...	Line 281...
280	`imMarkerSpace = rgb2gray(im2double(imMarkerSpace));`	281	`imMarkerSpace = rgb2gray(im2double(imMarkerSpace));`
281		282
282	`%figure; imshowpair(imMarkerSpace, m);`	283	`%figure; imshowpair(imMarkerSpace, m);`
283		284
284	`% perform image registration`	285	`% perform image registration`
-		286	`% might be better off using subpixel image correlation`
285	`[opt, met] = imregconfig('monomodal');`	287	`[opt, met] = imregconfig('multimodal');`
286	`T = imregtform(m, imMarkerSpace, 'translation', opt, met);`	288	`T = imregtform(m, imMarkerSpace, 'translation', opt, met, 'DisplayOptimization', true);`
287		289
288	`rege = imwarp(m, T, 'OutputView', imref2d(size(m)));`	290	`rege = imwarp(m, T, 'OutputView', imref2d(size(m)));`
289	`%figure; imshowpair(imMarkerSpace, rege);`	291	`%figure; imshowpair(imMarkerSpace, rege);`
290		292
291	`% measure of correlation`	293	`% measure of correlation`

Subversion Repositories seema-scanner

(root)/matlab/alignSubScansMarkers.m – Rev 213 → 214