forked from intellhave/SDRSAC
-
Notifications
You must be signed in to change notification settings - Fork 0
/
SDRSAC.m
88 lines (67 loc) · 2.89 KB
/
SDRSAC.m
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
% Main SDRSAC Algorithm
% Input:
% M, B: Input point clouds, each is a 3xN matrix
% config: Contains all parameters required to run the algoirthm.
% See readConfig.m for more info
% Output: a structure out containing output variables:
% out.bestR: Best rotation matrix
% out.bestT: Best translation vector
function [out] = SDRSAC(M, B, config)
add_dependencies;
ps = 0.99;
iter=0;
T_max = 1e10;
% Using KDTree for quick computing of consensus size
B_Tree = KDTreeSearcher(B');
% Prepare sampling
n = config.pointPerSample; % N_sample
maxInls = 0;
bestR = [];
bestT = [];
% Start the sampling iterations
stop = false;
while (iter < config.maxIter && ~stop)
idxM = randsample(size(M,2), n);
m = M(:, idxM);
B_to_sample = B;
scount = 0;
while (size(B_to_sample,2) > n*2 && scount < 2)
scount = scount + 1;
fprintf('Current B size: %d\n', size(B_to_sample,2));
idxB = randsample(size(B_to_sample,2), n);
b = B_to_sample(:, idxB);
B_to_sample(:,idxB) = [];
% Solve SDP
[Rs,ts, ~, corrB] = sdpReg(m, b, config);
if ~isempty(corrB)
TM = Rs*M + repmat(ts, 1, size(M,2));
% Conducting ICP
[Ricp, Ticp] = icp(B, TM, 'Matching', 'kDtree', 'WorstRejection', 0.1, 'iter', 100);
TMICP = Ricp*Rs*M + repmat(Ricp*ts + Ticp, 1, size(M, 2));
inls_icp = countCorrespondences(TMICP, B_Tree, config.epsilon);
if inls_icp > maxInls
maxInls = inls_icp;
bestR = Ricp*Rs;
bestT = Ricp*ts + Ticp;
fprintf('Best-so-far consensus size: %d\n', maxInls);%
fprintf('--------------');
% For debugging purpose:
%close all; plotPointClouds(B, TMICP, 'b.','r.');
% Compute stopping criterion
pI = maxInls./size(M,2);
T_max = log(1-ps)./log(1-pI^config.k);
end
end
iter = iter+1;
% Stopping criterion is satisfied (minimum of 5 iterations)
if iter >= T_max && iter >=5
stop = true;
end
end
end
out.inls = maxInls;
out.iter = iter;
out.R = bestR;
out.T = bestT;
plotPointClouds(B, bestR*M + bestT, 'b.','r.')
end