1%% Reset everything
2
3clear all;
4clc;
5close all;
6addpath('helpers');
7
8%% Configure the benchmark
9
10% central case -> only one camera
11cam_number = 1;
12% Getting 10 points, and testing all algorithms with the respective number of points
13pt_number = 10;
14% noise test, so no outliers
15outlier_fraction = 0.0;
16% repeat 5000 tests per noise level
17iterations = 5000;
18
19% The algorithms we want to test
20algorithms = { 'fivept_stewenius'; 'fivept_nister'; 'fivept_kneip'; 'sevenpt'; 'eightpt'; 'eigensolver'; 'rel_nonlin_central' };
21% Some parameter that tells us what the result means
22returns = [ 1, 1, 0, 1, 1, 0, 2 ]; % 1means essential matrix(ces) needing decomposition, %0 means rotation matrix(ces), %2 means transformation matrix
23% This defines the number of points used for every algorithm
24indices = { [1, 2, 3, 4, 5]; [1, 2, 3, 4, 5]; [1, 2, 3, 4, 5]; [1, 2, 3, 4, 5, 6, 7]; [1, 2, 3, 4, 5, 6, 7, 8]; [1, 2, 3, 4, 5, 6, 7, 8, 9, 10]; [1, 2, 3, 4, 5, 6, 7, 8, 9, 10] };
25% The name of the algorithms in the final plots
26names = { '5pt (Stewenius)'; '5pt (Nister)'; '5pt (Kneip)'; '7pt'; '8pt'; 'eigensolver (10pts)'; 'nonlin. opt. (10pts)' };
27
28% The maximum noise to analyze
29max_noise = 5.0;
30% The step in between different noise levels
31noise_step = 0.1;
32
33%% Run the benchmark
34
35%prepare the overall result arrays
36number_noise_levels = max_noise / noise_step + 1;
37num_algorithms = size(algorithms,1);
38mean_rotation_errors = zeros(num_algorithms,number_noise_levels);
39median_rotation_errors = zeros(num_algorithms,number_noise_levels);
40noise_levels = zeros(1,number_noise_levels);
41
42%Run the experiment
43for n=1:number_noise_levels
44
45    noise = (n - 1) * noise_step;
46    noise_levels(1,n) = noise;
47    display(['Analyzing noise level: ' num2str(noise)])
48
49    rotation_errors = zeros(num_algorithms,iterations);
50
51    counter = 0;
52
53    validIterations = 0;
54
55    for i=1:iterations
56
57        % generate experiment
58        [v1,v2,t,R] = create2D2DExperiment(pt_number,cam_number,noise,outlier_fraction);
59        [t_perturbed,R_perturbed] = perturb(t,R,0.01);
60        T_perturbed = [R_perturbed,t_perturbed];
61        R_gt = R;
62
63        % run all algorithms
64        allValid = 1;
65
66        for a=1:num_algorithms
67            Out = opengv(algorithms{a},indices{a},v1,v2,T_perturbed);
68
69            if ~isempty(Out)
70
71                if returns(1,a) == 1
72                    temp = transformEssentials(Out);
73                    Out = temp;
74                end
75                if returns(1,a) == 2
76                    temp = Out(:,1:3);
77                    Out = temp;
78                end
79
80                rotation_errors(a,validIterations+1) = evaluateRotationError( R_gt, Out );
81
82            else
83
84                allValid = 0;
85                break;
86
87            end
88        end
89
90        if allValid == 1
91            validIterations = validIterations +1;
92        end
93
94        counter = counter + 1;
95        if counter == 100
96            counter = 0;
97            display(['Iteration ' num2str(i) ' of ' num2str(iterations) '(noise level ' num2str(noise) ')']);
98        end
99    end
100
101    %Now compute the mean and median value of the error for each algorithm
102    for a=1:num_algorithms
103        mean_rotation_errors(a,n) = mean(rotation_errors(a,1:validIterations));
104        median_rotation_errors(a,n) = median(rotation_errors(a,1:validIterations));
105    end
106
107end
108
109%% Plot the results
110
111figure(1)
112plot(noise_levels,mean_rotation_errors,'LineWidth',2)
113legend(names,'Location','NorthWest')
114xlabel('noise level [pix]')
115ylabel('mean rot. error [rad]')
116grid on
117
118figure(2)
119plot(noise_levels,median_rotation_errors,'LineWidth',2)
120legend(names,'Location','NorthWest')
121xlabel('noise level [pix]')
122ylabel('median rot. error [rad]')
123grid on
124