1%% Reset everything
2
3clear all;
4clc;
5close all;
6addpath('helpers');
7
8%% Configure the benchmark
9
10% noncentral case
11cam_number = 4;
12% let's only get 6 points, and generate new ones in each iteration
13pt_number = 50;
14% noise test, so no outliers
15outlier_fraction = 0.0;
16% repeat 5000 iterations per noise level
17iterations = 5000;
18
19% The algorithms we want to test
20algorithms = { 'gp3p'; 'gpnp'; 'gpnp'; 'abs_nonlin_noncentral'; 'abs_nonlin_noncentral'; 'upnp'; 'upnp' };
21% This defines the number of points used for every algorithm
22indices = { [1, 2, 3]; [1:1:10]; [1:1:50]; [1:1:10]; [1:1:50]; [1:1:10]; [1:1:50] };
23% The name of the algorithms on the plots
24names = { 'GP3P'; 'GPnP (10pts)'; 'GPnP (50pts)'; 'nonlin. opt. (10pts)'; 'nonlin. opt. (50pts)'; 'UPnP (10pts)'; 'UPnP (50pts)' };
25
26% The maximum noise to analyze
27max_noise = 5.0;
28% The step in between different noise levels
29noise_step = 0.1;
30
31%% Run the benchmark
32
33%prepare the overall result arrays
34number_noise_levels = max_noise / noise_step + 1;
35num_algorithms = size(algorithms,1);
36mean_position_errors = zeros(num_algorithms,number_noise_levels);
37mean_rotation_errors = zeros(num_algorithms,number_noise_levels);
38median_position_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    position_errors = zeros(num_algorithms,iterations);
50    rotation_errors = zeros(num_algorithms,iterations);
51
52    counter = 0;
53
54    validIterations = 0;
55
56    for i=1:iterations
57
58        % generate experiment
59        [points,v,t,R] = create2D3DExperiment(pt_number,cam_number,noise,outlier_fraction);
60        [t_perturbed,R_perturbed] = perturb(t,R,0.01);
61        T_perturbed = [R_perturbed,t_perturbed];
62        T_gt = [R,t];
63
64        % run all algorithms
65        allValid = 1;
66
67        for a=1:num_algorithms
68            T = opengv(algorithms{a},indices{a},points,v,T_perturbed);
69            [position_error, rotation_error] = evaluateTransformationError( T_gt, T );
70
71            if( position_error > 100 )
72                allValid = 0;
73                break;
74            else
75                position_errors(a,validIterations+1) = position_error;
76                rotation_errors(a,validIterations+1) = rotation_error;
77            end
78        end
79
80        if allValid == 1
81            validIterations = validIterations +1;
82        end
83
84        counter = counter + 1;
85        if counter == 100
86            counter = 0;
87            display(['Iteration ' num2str(i) ' of ' num2str(iterations) '(noise level ' num2str(noise) ')']);
88        end
89    end
90
91    %Now compute the mean and median value of the error for each algorithm
92    for a=1:num_algorithms
93        mean_position_errors(a,n) = mean(position_errors(a,1:validIterations));
94        median_position_errors(a,n) = median(position_errors(a,1:validIterations));
95        mean_rotation_errors(a,n) = mean(rotation_errors(a,1:validIterations));
96        median_rotation_errors(a,n) = median(rotation_errors(a,1:validIterations));
97    end
98
99end
100
101%% Plot the results
102
103figure(1)
104plot(noise_levels',mean_rotation_errors','LineWidth',2)
105legend(names,'Location','NorthWest')
106xlabel('noise level [pix]')
107ylabel('mean rot. error [rad]')
108grid on
109
110figure(2)
111plot(noise_levels',median_rotation_errors','LineWidth',2)
112legend(names,'Location','NorthWest')
113xlabel('noise level [pix]')
114ylabel('median rot. error [rad]')
115grid on
116
117figure(3)
118plot(noise_levels',mean_position_errors','LineWidth',2)
119legend(names,'Location','NorthWest')
120xlabel('noise level [pix]')
121ylabel('mean pos. error [m]')
122grid on
123
124figure(4)
125plot(noise_levels',median_position_errors','LineWidth',2)
126legend(names,'Location','NorthWest')
127xlabel('noise level [pix]')
128ylabel('median pos. error [m]')
129grid on