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)