1%% Reset everything
2
3clear all;
4clc;
5close all;
6addpath('helpers');
7
8%% Configure the benchmark
9
10% noncentral case
11cam_number = 4;
12% Getting 20 points
13pt_number = 20;
14% no outliers
15outlier_fraction = 0.0;
16% no noise
17noise = 0.0;
18
19[v1,v2,t,R] = create2D2DOmniExperiment(pt_number,cam_number,noise,outlier_fraction);
20
21
22%% Plot the smallest Eigenvalue
23
24evs = zeros(101,101);
25
26for cay_z_index=1:5
27
28    cay_z = (cay_z_index - 3) * 0.2;
29    cay_z_index
30
31    for i=1:101
32        cay_x = 0.4*((i-51)/50);
33        i
34        for j=1:101
35
36            cay_y = 0.4*((j-51) / 50);
37
38            x = [cay_x;cay_y;cay_z];
39
40            if i == 51 && j == 51 && cay_z_index == 3
41                cay_y = 0.4*((52-51) / 50);
42                x = [cay_x;cay_y;cay_z];
43            end
44
45            Out = opengv('ge2',[1:1:size(v1,2)],v1,v2,[cayley2rot(x) zeros(3,1)]);
46            evs(i,j) = Out(4,5);
47
48        end
49    end
50
51    negativeIndices = evs<0;
52    evs(negativeIndices) = 0.0;
53
54    figure(1)
55    hold on
56    surf([-0.4 0.4],[-0.4 0.4],repmat(cay_z, [2 2]),log(evs),'facecolor','texture')
57    %colormap(gray);
58    view(45,30);
59    daspect([2.5 2.5 1]);
60    axis([-0.4 0.4 -0.4 0.4 -0.4 0.4])
61    grid on
62    xlabel('x');
63    ylabel('y');
64    zlabel('z');
65    colorbar
66
67end
68
69%% insert the zero layer
70insertZeroLayer = 0;
71
72if insertZeroLayer == 1
73
74    gt = rot2cayley(R);
75    cay_z = gt(3,1);
76
77    for i=1:101
78        cay_x = 0.4*((i-51)/50);
79        i
80        for j=1:101
81
82            cay_y = 0.4*((j-51) / 50);
83
84            x = [cay_x;cay_y;cay_z];
85
86            if i == 51 && j == 51 && cay_z_index == 3
87                cay_y = 0.4*((52-51) / 50);
88                x = [cay_x;cay_y;cay_z];
89            end
90
91            Out = opengv('ge2',[1:1:size(v1,2)],v1,v2,[cayley2rot(x) zeros(3,1)]);
92            evs(i,j) = Out(4,5);
93
94        end
95    end
96
97    negativeIndices = evs<0;
98    evs(negativeIndices) = 0.0;
99
100    figure(1)
101    hold on
102    surf([-0.4 0.4],[-0.4 0.4],repmat(cay_z, [2 2]),log(evs),'facecolor','texture')
103    %colormap(gray);
104    view(45,30);
105    daspect([2.5 2.5 1]);
106    axis([-0.4 0.4 -0.4 0.4 -0.4 0.4])
107    grid on
108    xlabel('x');
109    ylabel('y');
110    zlabel('z');
111    colorbar
112
113end
114
115%% Plot the second smallest Eigenvalue
116
117evs = zeros(101,101);
118
119for cay_z_index=1:5
120
121    cay_z = (cay_z_index - 3) * 0.2;
122    cay_z_index
123
124    for i=1:101
125        cay_x = 0.4*((i-51)/50);
126        i
127        for j=1:101
128
129            cay_y = 0.4*((j-51) / 50);
130
131            x = [cay_x;cay_y;cay_z];
132
133            if i == 51 && j == 51 && cay_z_index == 3
134                cay_y = 0.4*((52-51) / 50);
135                x = [cay_x;cay_y;cay_z];
136            end
137
138            Out = opengv('ge2',[1:1:size(v1,2)],v1,v2,[cayley2rot(x) zeros(3,1)]);
139            evs(i,j) = Out(3,5);
140
141        end
142    end
143
144    negativeIndices = evs<0;
145    evs(negativeIndices) = 0.0;
146
147    figure(2)
148    hold on
149    surf([-0.4 0.4],[-0.4 0.4],repmat(cay_z, [2 2]),log(evs),'facecolor','texture')
150    %colormap(gray);
151    view(45,30);
152    daspect([2.5 2.5 1]);
153    axis([-0.4 0.4 -0.4 0.4 -0.4 0.4])
154    grid on
155    xlabel('x');
156    ylabel('y');
157    zlabel('z');
158    colorbar
159
160end
161
162%% insert the zero layer
163insertZeroLayer = 0;
164
165if insertZeroLayer == 1
166
167    gt = rot2cayley(R);
168    cay_z = gt(3,1);
169
170    for i=1:101
171        cay_x = 0.4*((i-51)/50);
172        i
173        for j=1:101
174
175            cay_y = 0.4*((j-51) / 50);
176
177            x = [cay_x;cay_y;cay_z];
178
179            if i == 51 && j == 51 && cay_z_index == 3
180                cay_y = 0.4*((52-51) / 50);
181                x = [cay_x;cay_y;cay_z];
182            end
183
184            Out = opengv('ge2',[1:1:size(v1,2)],v1,v2,[cayley2rot(x) zeros(3,1)]);
185            evs(i,j) = Out(3,5);
186
187        end
188    end
189
190    negativeIndices = evs<0;
191    evs(negativeIndices) = 0.0;
192
193    figure(2)
194    hold on
195    surf([-0.4 0.4],[-0.4 0.4],repmat(cay_z, [2 2]),log(evs),'facecolor','texture')
196    %colormap(gray);
197    view(45,30);
198    daspect([2.5 2.5 1]);
199    axis([-0.4 0.4 -0.4 0.4 -0.4 0.4])
200    grid on
201    xlabel('x');
202    ylabel('y');
203    zlabel('z');
204    colorbar
205
206end
207
208%% Print the ground truth value on the console
209
210minimum = rot2cayley(R)