-
Notifications
You must be signed in to change notification settings - Fork 2
/
RANSAC_CALC_VER_test.m
174 lines (161 loc) · 7.08 KB
/
RANSAC_CALC_VER_test.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
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
149
150
151
152
153
154
155
156
157
158
159
160
161
162
163
164
165
166
167
168
169
function [R,T,error,BestFit]=RANSAC_CALC_VER2(Ya,Yb,options)
%%% Implementation of the RANSAC algorithm of the paper
% @INPROCEEDINGS {gpandey-2011b,
% AUTHOR = { Gaurav Pandey and James R. McBride and Silvio Savarese and Ryan M. Eustice },
% TITLE = { Visually Bootstrapped Generalized ICP },
% BOOKTITLE = { Proceedings of the IEEE International Conference on Robotics and Automation },
% YEAR = { 2011 },
% ADDRESS = { Shanghai, China },
% NOTE = { Accepted, To Appear },
% }
% 1: input: YA, YB, SA, SB,
% 2: output: The estimated transformation [R0; t0]
% 3: Establish camera constrained SIFT correspondences between
% SA and SB.
% 4: Store the matches in a list L.
% 5: while iter < MAXITER do
% 6: Randomly pick 3 pairs of points from the list L.
% 7: Retrieve these 3 pair of points from YA and YB.
% 8: Calculate the 6-DOF rigid body transformation [R; t]
% that best aligns these 3 points.
% 9: Store this transformation in an array M, M[iter] =
% [R; t]
% 10: Apply the transformation to YB to map Scan B�s
% points into the reference frame of Scan A: Y0
% B = RYB + t
% 11: Calculate the set cardinality of pose-consistent SIFT
% correspondences that agree with the current transformation
% (i.e., those that satisfy a Euclidean threshold
% on spatial proximity): n = j(Y0
% B (L) ? YA(L)) < j
% 12: Store the number of pose-consistent correspondences
% in an array N, N[iter] = n
% 13: iter = iter + 1
% 14: end while
% 15: Find the index i that has maximum number of correspondences
% in N.
% 16: Retrieve the transformation corresponding to index i
% 17: Recalculate the Transformation and Rotation
% from M. [R0; T0] = M[i]. This is the required transformation.
epsilon = 0.01; %% confidence interval (1-epsilon) is the probability that
% we draw a set from the all samples that consists of all inliers
nIterations = options.MaxIteration;
maxSupport = 5; %% since the size of the set which produces any hypothesis
% is 5 for each hypothesis we have at least 5 inliers
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% matches matrix has the following structure
% Yb Currnet Frame matches First Row
% Ya Previous Frame matches Second Row
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
iter=1;
Y0b=zeros(size(Yb));
M=struct('R',[],'T',[],'ErrorSum',[]);
Cardinality=zeros(1,options.MaxIteration);
ErrorSum = 0;
nPoints = size(Yb,2);
nSetHypothesisGenerator =5;
while iter<min(nIterations,options.MaxIteration) %% this value is updated during each loop running
% 6: Randomly pick 3 pairs of points from the list L.
RandIndex = get_rand(nSetHypothesisGenerator, size(Ya,2) ); %% Remember that indices should not be the same
% 7: Retrieve these 3 pair of points from YA and YB.
SetPoints_a=Ya(:,RandIndex); %% Point sets should be in the form P={P1,P2,..}
SetPoints_b=Yb(:,RandIndex); %%
% 8: Calculate the 6-DOF rigid body transformation [R; t]
% that best aligns these 3 points.
%% DEBUG comparing the results of current transformation calculation code with the results of quaternion based calculation
% [Rot, Trans, State] = find_transform_matrix(SetPoints_a, SetPoints_b);
%%%% absoluteOrientationQuaternion(Current frame,previos frame)
[s, Rot, Trans, err] = absoluteOrientationQuaternion(SetPoints_b, SetPoints_a, 0);
%%
%% DEBUG
if ~isreal(Rot) || abs(det(Rot)-1)>0.0001
disp('Rotation matrix contains imaginary values (Degenerate case)')
end
% [RR1, TT1] = icp(SetPoints_a',SetPoints_b');
% [RR2, TT2] = icp(Ya,Yb);
% 9: Store this transformation in an array M, M[iter] =
% [R; t]
M(iter).R=Rot;
M(iter).T=Trans;
M(iter).ErrorSum=0;
% 10: Apply the transformation to YB to map Scan B�s
% points into the reference frame of Scan A: Y0B = RYB + t
%%% For later usage we need to keep the support set of each Hypothesis.
%%% This is especially useful in recalculation step
M(iter).SupportSet.Ya=[];
M(iter).SupportSet.Yb=[];
Y0b=Rot*Yb+repmat(Trans,1,size(Yb,2));
residu = Y0b-Ya;
normResidu = sqrt(residu(1,:).^2+residu(2,:).^2+residu(3,:).^2);
position_inliers = normResidu<options.DistanceThreshold;
Cardinality(iter) = sum(position_inliers);
M(iter).SupportSet.Ya = Ya(:,position_inliers);
M(iter).SupportSet.Yb = Yb(:,position_inliers);
M(iter).ErrorSum = sum(normResidu(position_inliers));
if Cardinality(iter)>=maxSupport
maxSupport = Cardinality(iter);
nIterations = ceil(log(epsilon)/log( 1-(Cardinality(iter)/nPoints)^nSetHypothesisGenerator ) );
end
% for (i=1:size(Ya,2))
% try
% Y0b(:,i)=Rot*Yb(:,i)+Trans;
% catch
% disp('Salam Guguli')
% end
% % 11: Calculate the set cardinality of pose-consistent SIFT
% % correspondences that agree with the current transformation
% % (i.e., those that satisfy an Euclidean threshold
% % on spatial proximity): n = norm(Y0b(L) - YA(L)) < epsilon
% % 12: Store the number of pose-consistent correspondences
% % in an array N, N[iter] = n
% TempError=norm(Y0b(:,i)-Ya(:,i));
% M(iter).ErrorSum = M(iter).ErrorSum+ TempError;
% if TempError<=options.DistanceThreshold
% M(iter).SupportSet.Ya=[M(iter).SupportSet.Ya,Ya(:,i)];
% M(iter).SupportSet.Yb=[M(iter).SupportSet.Yb,Yb(:,i)];
% Cardinality(iter)=Cardinality(iter)+1;
% end
% end
iter=iter+1;
end
% 15: Find the index "iter" that has maximum number of correspondences
% in N.
for iii=1:length(M)
eee1(iii)=M(iii).ErrorSum;
eee2(iii)=Cardinality(iii);
end
MaxCardinality=max(eee2);
eee2(eee2~=max(eee2))=0;
eee1(eee2==0)=10000;
[C,I] = min(eee1);
BestError=C;
BestFitIdx=I;
BestFit=MaxCardinality;
% [BestFit BestFitIdx]=max(Cardinality);
% 16: Retrieve the transformation corresponding to index i
% from M. [R0; T0] = M[i]. This is the required transformation.
R= M(BestFitIdx).R;
T= M(BestFitIdx).T;
% 17: Recalculate the Transformation and Rotationfor the best hypothesis
%% DEBUG
% This is the original code that I used but it seems that it fails in some situations (degenerate)
% [R, T, State] = find_transform_matrix(M(BestFitIdx).SupportSet.Ya, M(BestFitIdx).SupportSet.Yb);
% I want to test using quaternion based calculation
%%%% absoluteOrientationQuaternion(Current frame (Yb),previos frame(Ya))
[s, R, T, err] = absoluteOrientationQuaternion(M(BestFitIdx).SupportSet.Yb, M(BestFitIdx).SupportSet.Ya, 0);
%%
if ~isreal(R) || abs(det(R)-1)>0.0001
disp('Rotation matrix contains imaginary values (Degenerate case)')
end
%% DEBUG
if norm(T)>0.12
disp('large distance traveled is this OK?')
end
%%
error=M(BestFitIdx).ErrorSum;
error.mYa =M(BestFitIdx).SupportSet.Ya;
error.mYb =M(BestFitIdx).SupportSet.Yb;
disp(['max iter = ' num2str(length(M)) ])
% 18: covariance calculation
% covariance = covariance_estimate_RANSAC(M(BestFitIdx).SupportSet.Ya, M(BestFitIdx).SupportSet.Yb,R,T);
% covariance = cov_est_RANSAC_deriv(M(BestFitIdx).SupportSet.Ya, M(BestFitIdx).SupportSet.Yb,R,T);