ml/ar_basalt
Archived
1
0
Fork 0
Code Issues Pull Requests Releases Wiki Activity

v01

Browse Source
This commit is contained in:
Ivan
2022-04-05 11:42:28 +03:00
commit 6dc0eb0fcf
5565 changed files with 1200500 additions and 0 deletions
Download Patch File Download Diff File Expand all files Collapse all files

115
thirdparty/opengv/matlab/benchmark_absolute_pose.m vendored Normal file
View File

@@ -0,0 +1,115 @@
%% Reset everything
clear all;
clc;
close all;
addpath('helpers');
%% Configure the benchmark
% central case -> only one camera
cam_number = 1;
% let's only get 6 points, and generate new ones in each iteration
pt_number = 6;
% noise test, so no outliers
outlier_fraction = 0.0;
% repeat 5000 iterations per noise level
iterations = 5000;
% The algorithms we want to test
algorithms = { 'p3p_kneip'; 'p3p_gao'; 'epnp'; 'abs_nonlin_central'; 'upnp'; 'upnp' };
% This defines the number of points used for every algorithm
indices = { [1, 2, 3]; [1, 2, 3]; [1, 2, 3, 4, 5, 6]; [1, 2, 3, 4, 5, 6]; [1, 2, 3, 4, 5, 6]; [1, 2, 3] };
% The name of the algorithms on the plots
names = { 'P3P (Kneip)'; 'P3P (Gao)'; 'EPnP'; 'nonlinear optimization'; 'UPnP'; 'UPnP (minimal)' };
% The maximum noise to analyze
max_noise = 5.0;
% The step in between different noise levels
noise_step = 0.1;
%% Run the benchmark
%prepare the overall result arrays
number_noise_levels = max_noise / noise_step + 1;
num_algorithms = size(algorithms,1);
mean_position_errors = zeros(num_algorithms,number_noise_levels);
mean_rotation_errors = zeros(num_algorithms,number_noise_levels);
median_position_errors = zeros(num_algorithms,number_noise_levels);
median_rotation_errors = zeros(num_algorithms,number_noise_levels);
noise_levels = zeros(1,number_noise_levels);
%Run the experiment
for n=1:number_noise_levels
noise = (n - 1) * noise_step;
noise_levels(1,n) = noise;
display(['Analyzing noise level: ' num2str(noise)])
position_errors = zeros(num_algorithms,iterations);
rotation_errors = zeros(num_algorithms,iterations);
counter = 0;
for i=1:iterations
% generate experiment
[points,v,t,R] = create2D3DExperiment(pt_number,cam_number,noise,outlier_fraction);
[t_perturbed,R_perturbed] = perturb(t,R,0.01);
T_perturbed = [R_perturbed,t_perturbed];
T_gt = [R,t];
% run all algorithms
for a=1:num_algorithms
T = opengv(algorithms{a},indices{a},points,v,T_perturbed);
[position_error, rotation_error] = evaluateTransformationError( T_gt, T );
position_errors(a,i) = position_error;
rotation_errors(a,i) = rotation_error;
end
counter = counter + 1;
if counter == 100
counter = 0;
display(['Iteration ' num2str(i) ' of ' num2str(iterations) '(noise level ' num2str(noise) ')']);
end
end
%Now compute the mean and median value of the error for each algorithm
for a=1:num_algorithms
mean_position_errors(a,n) = mean(position_errors(a,:));
median_position_errors(a,n) = median(position_errors(a,:));
mean_rotation_errors(a,n) = mean(rotation_errors(a,:));
median_rotation_errors(a,n) = median(rotation_errors(a,:));
end
end
%% Plot the results
figure(1)
plot(noise_levels',mean_rotation_errors','LineWidth',2)
legend(names,'Location','NorthWest')
xlabel('noise level [pix]')
ylabel('mean rot. error [rad]')
grid on
figure(2)
plot(noise_levels',median_rotation_errors','LineWidth',2)
legend(names,'Location','NorthWest')
xlabel('noise level [pix]')
ylabel('median rot. error [rad]')
grid on
figure(3)
plot(noise_levels',mean_position_errors','LineWidth',2)
legend(names,'Location','NorthWest')
xlabel('noise level [pix]')
ylabel('mean pos. error [m]')
grid on
figure(4)
plot(noise_levels',median_position_errors','LineWidth',2)
legend(names,'Location','NorthWest')
xlabel('noise level [pix]')
ylabel('median pos. error [m]')
grid on

65
thirdparty/opengv/matlab/benchmark_absolute_pose_execution_times.m vendored Normal file
View File

@@ -0,0 +1,65 @@
%% Reset everything
clear all;
clc;
close all;
addpath('helpers');
%% Configure the benchmark
% central case -> only one camera
cam_number = 1;
% let's only get 6 points, and generate new ones in each iteration
pt_number = 50;
% noise test, so no outliers
outlier_fraction = 0.0;
% repeat 1000 iterations
iterations = 1000;
% The algorithms we want to test
algorithms = { 'p2p'; 'p3p_kneip'; 'p3p_gao'; 'epnp'; 'upnp' };
% This defines the number of points used for every algorithm
indices = { [1, 2]; [1, 2, 3]; [1, 2, 3]; [1:1:50]; [1:1:50] };
% The name of the algorithms on the plots
names = { 'P2P'; 'P3P (Kneip)'; 'P3P (Gao)'; 'EPnP (50pts)'; 'UPnP (50pts)'};
% The noise in this experiment
noise = 1.0;
%% Run the benchmark
%prepare the overall result array
num_algorithms = size(algorithms,1);
execution_times = zeros(num_algorithms,iterations);
counter = 0;
for i=1:iterations
% generate experiment
[points,v,t,R] = create2D3DExperiment(pt_number,cam_number,noise,outlier_fraction);
[t_perturbed,R_perturbed] = perturb(t,R,0.01);
T_perturbed = [R_perturbed,t_perturbed];
% run all algorithms
for a=1:num_algorithms
tic;
T = opengv_donotuse(algorithms{a},indices{a},points,v,T_perturbed);
execution_times(a,i) = toc/20.0;
end
counter = counter + 1;
if counter == 100
counter = 0;
display(['Iteration ' num2str(i) ' of ' num2str(iterations)]);
end
end
%% Plot the results
bins = [0.000001:0.000001:0.00001];
hist(execution_times',bins)
legend(names,'Location','NorthWest')
xlabel('execution times [s]')
grid on
mean(execution_times')

129
thirdparty/opengv/matlab/benchmark_absolute_pose_noncentral.m vendored Normal file
View File

@@ -0,0 +1,129 @@
%% Reset everything
clear all;
clc;
close all;
addpath('helpers');
%% Configure the benchmark
% noncentral case
cam_number = 4;
% let's only get 6 points, and generate new ones in each iteration
pt_number = 50;
% noise test, so no outliers
outlier_fraction = 0.0;
% repeat 5000 iterations per noise level
iterations = 5000;
% The algorithms we want to test
algorithms = { 'gp3p'; 'gpnp'; 'gpnp'; 'abs_nonlin_noncentral'; 'abs_nonlin_noncentral'; 'upnp'; 'upnp' };
% This defines the number of points used for every algorithm
indices = { [1, 2, 3]; [1:1:10]; [1:1:50]; [1:1:10]; [1:1:50]; [1:1:10]; [1:1:50] };
% The name of the algorithms on the plots
names = { 'GP3P'; 'GPnP (10pts)'; 'GPnP (50pts)'; 'nonlin. opt. (10pts)'; 'nonlin. opt. (50pts)'; 'UPnP (10pts)'; 'UPnP (50pts)' };
% The maximum noise to analyze
max_noise = 5.0;
% The step in between different noise levels
noise_step = 0.1;
%% Run the benchmark
%prepare the overall result arrays
number_noise_levels = max_noise / noise_step + 1;
num_algorithms = size(algorithms,1);
mean_position_errors = zeros(num_algorithms,number_noise_levels);
mean_rotation_errors = zeros(num_algorithms,number_noise_levels);
median_position_errors = zeros(num_algorithms,number_noise_levels);
median_rotation_errors = zeros(num_algorithms,number_noise_levels);
noise_levels = zeros(1,number_noise_levels);
%Run the experiment
for n=1:number_noise_levels
noise = (n - 1) * noise_step;
noise_levels(1,n) = noise;
display(['Analyzing noise level: ' num2str(noise)])
position_errors = zeros(num_algorithms,iterations);
rotation_errors = zeros(num_algorithms,iterations);
counter = 0;
validIterations = 0;
for i=1:iterations
% generate experiment
[points,v,t,R] = create2D3DExperiment(pt_number,cam_number,noise,outlier_fraction);
[t_perturbed,R_perturbed] = perturb(t,R,0.01);
T_perturbed = [R_perturbed,t_perturbed];
T_gt = [R,t];
% run all algorithms
allValid = 1;
for a=1:num_algorithms
T = opengv(algorithms{a},indices{a},points,v,T_perturbed);
[position_error, rotation_error] = evaluateTransformationError( T_gt, T );
if( position_error > 100 )
allValid = 0;
break;
else
position_errors(a,validIterations+1) = position_error;
rotation_errors(a,validIterations+1) = rotation_error;
end
end
if allValid == 1
validIterations = validIterations +1;
end
counter = counter + 1;
if counter == 100
counter = 0;
display(['Iteration ' num2str(i) ' of ' num2str(iterations) '(noise level ' num2str(noise) ')']);
end
end
%Now compute the mean and median value of the error for each algorithm
for a=1:num_algorithms
mean_position_errors(a,n) = mean(position_errors(a,1:validIterations));
median_position_errors(a,n) = median(position_errors(a,1:validIterations));
mean_rotation_errors(a,n) = mean(rotation_errors(a,1:validIterations));
median_rotation_errors(a,n) = median(rotation_errors(a,1:validIterations));
end
end
%% Plot the results
figure(1)
plot(noise_levels',mean_rotation_errors','LineWidth',2)
legend(names,'Location','NorthWest')
xlabel('noise level [pix]')
ylabel('mean rot. error [rad]')
grid on
figure(2)
plot(noise_levels',median_rotation_errors','LineWidth',2)
legend(names,'Location','NorthWest')
xlabel('noise level [pix]')
ylabel('median rot. error [rad]')
grid on
figure(3)
plot(noise_levels',mean_position_errors','LineWidth',2)
legend(names,'Location','NorthWest')
xlabel('noise level [pix]')
ylabel('mean pos. error [m]')
grid on
figure(4)
plot(noise_levels',median_position_errors','LineWidth',2)
legend(names,'Location','NorthWest')
xlabel('noise level [pix]')
ylabel('median pos. error [m]')
grid on

63
thirdparty/opengv/matlab/benchmark_absolute_pose_noncentral_execution_timing.m vendored Normal file
View File

@@ -0,0 +1,63 @@
%% Reset everything
clear all;
clc;
close all;
addpath('helpers');
%% Configure the benchmark
% central case -> only one camera
cam_number = 4;
% let's only get 6 points, and generate new ones in each iteration
pt_number = 50;
% noise test, so no outliers
outlier_fraction = 0.0;
% repeat 1000 iterations
iterations = 1000;
% The algorithms we want to test
algorithms = { 'gp3p'; 'gpnp'; 'upnp'};
% This defines the number of points used for every algorithm
indices = { [1, 2, 3]; [1:1:50]; [1:1:50] };
% The name of the algorithms on the plots
names = { 'GP3P'; 'GPnP (50pts)'; 'UPnP (50pts)' };
% The noise in this experiment
noise = 1.0;
%% Run the benchmark
%prepare the overall result arrays
num_algorithms = size(algorithms,1);
execution_times = zeros(num_algorithms,iterations);
counter = 0;
for i=1:iterations
% generate experiment
[points,v,t,R] = create2D3DExperiment(pt_number,cam_number,noise,outlier_fraction);
[t_perturbed,R_perturbed] = perturb(t,R,0.01);
T_perturbed = [R_perturbed,t_perturbed];
for a=1:num_algorithms
tic
T = opengv_donotuse(algorithms{a},indices{a},points,v,T_perturbed);
execution_times(a,i) = toc/20.0;
end
counter = counter + 1;
if counter == 100
counter = 0;
display(['Iteration ' num2str(i) ' of ' num2str(iterations) '(noise level ' num2str(noise) ')']);
end
end
%% Plot the results
hist(execution_times')
legend(names,'Location','NorthWest')
xlabel('execution times [s]')
grid on
mean(execution_times')

123
thirdparty/opengv/matlab/benchmark_relative_pose.m vendored Normal file
View File

@@ -0,0 +1,123 @@
%% Reset everything
clear all;
clc;
close all;
addpath('helpers');
%% Configure the benchmark
% central case -> only one camera
cam_number = 1;
% Getting 10 points, and testing all algorithms with the respective number of points
pt_number = 10;
% noise test, so no outliers
outlier_fraction = 0.0;
% repeat 5000 tests per noise level
iterations = 5000;
% The algorithms we want to test
algorithms = { 'fivept_stewenius'; 'fivept_nister'; 'fivept_kneip'; 'sevenpt'; 'eightpt'; 'eigensolver'; 'rel_nonlin_central' };
% Some parameter that tells us what the result means
returns = [ 1, 1, 0, 1, 1, 0, 2 ]; % 1means essential matrix(ces) needing decomposition, %0 means rotation matrix(ces), %2 means transformation matrix
% This defines the number of points used for every algorithm
indices = { [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] };
% The name of the algorithms in the final plots
names = { '5pt (Stewenius)'; '5pt (Nister)'; '5pt (Kneip)'; '7pt'; '8pt'; 'eigensolver (10pts)'; 'nonlin. opt. (10pts)' };
% The maximum noise to analyze
max_noise = 5.0;
% The step in between different noise levels
noise_step = 0.1;
%% Run the benchmark
%prepare the overall result arrays
number_noise_levels = max_noise / noise_step + 1;
num_algorithms = size(algorithms,1);
mean_rotation_errors = zeros(num_algorithms,number_noise_levels);
median_rotation_errors = zeros(num_algorithms,number_noise_levels);
noise_levels = zeros(1,number_noise_levels);
%Run the experiment
for n=1:number_noise_levels
noise = (n - 1) * noise_step;
noise_levels(1,n) = noise;
display(['Analyzing noise level: ' num2str(noise)])
rotation_errors = zeros(num_algorithms,iterations);
counter = 0;
validIterations = 0;
for i=1:iterations
% generate experiment
[v1,v2,t,R] = create2D2DExperiment(pt_number,cam_number,noise,outlier_fraction);
[t_perturbed,R_perturbed] = perturb(t,R,0.01);
T_perturbed = [R_perturbed,t_perturbed];
R_gt = R;
% run all algorithms
allValid = 1;
for a=1:num_algorithms
Out = opengv(algorithms{a},indices{a},v1,v2,T_perturbed);
if ~isempty(Out)
if returns(1,a) == 1
temp = transformEssentials(Out);
Out = temp;
end
if returns(1,a) == 2
temp = Out(:,1:3);
Out = temp;
end
rotation_errors(a,validIterations+1) = evaluateRotationError( R_gt, Out );
else
allValid = 0;
break;
end
end
if allValid == 1
validIterations = validIterations +1;
end
counter = counter + 1;
if counter == 100
counter = 0;
display(['Iteration ' num2str(i) ' of ' num2str(iterations) '(noise level ' num2str(noise) ')']);
end
end
%Now compute the mean and median value of the error for each algorithm
for a=1:num_algorithms
mean_rotation_errors(a,n) = mean(rotation_errors(a,1:validIterations));
median_rotation_errors(a,n) = median(rotation_errors(a,1:validIterations));
end
end
%% Plot the results
figure(1)
plot(noise_levels,mean_rotation_errors,'LineWidth',2)
legend(names,'Location','NorthWest')
xlabel('noise level [pix]')
ylabel('mean rot. error [rad]')
grid on
figure(2)
plot(noise_levels,median_rotation_errors,'LineWidth',2)
legend(names,'Location','NorthWest')
xlabel('noise level [pix]')
ylabel('median rot. error [rad]')
grid on

63
thirdparty/opengv/matlab/benchmark_relative_pose_execution_times.m vendored Normal file
View File

@@ -0,0 +1,63 @@
%% Reset everything
clear all;
clc;
close all;
addpath('helpers');
%% Configure the benchmark
% central case -> only one camera
cam_number = 1;
% Getting 10 points, and testing all algorithms with the respective number of points
pt_number = 10;
% noise test, so no outliers
outlier_fraction = 0.0;
% repeat 1000 iterations
iterations = 1000;
% The algorithms we want to test
algorithms = { 'twopt';'fivept_stewenius'; 'fivept_nister'; 'fivept_kneip'; 'sevenpt'; 'eightpt'; 'eigensolver' };
% This defines the number of points used for every algorithm
indices = { [1,2]; [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] };
% The name of the algorithms in the final plots
names = { '2pt';'5pt (Stewenius)'; '5pt (Nister)'; '5pt (Kneip)'; '7pt'; '8pt'; 'eigensolver' };
% noise in this experiment
noise = 1.0;
%% Run the benchmark
%prepare the overall result arrays
num_algorithms = size(algorithms,1);
execution_times = zeros(num_algorithms,iterations);
counter = 0;
for i=1:iterations
% generate experiment
[v1,v2,t,R] = create2D2DExperiment(pt_number,cam_number,noise,outlier_fraction);
[t_perturbed,R_perturbed] = perturb(t,R,0.01);
T_perturbed = [R_perturbed,t_perturbed];
for a=1:num_algorithms
tic
Out = opengv_donotuse(algorithms{a},indices{a},v1,v2,T_perturbed);
execution_times(a,i) = toc/20.0;
end
counter = counter + 1;
if counter == 100
counter = 0;
display(['Iteration ' num2str(i) ' of ' num2str(iterations)]);
end
end
%% Plot the results
hist(execution_times')
legend(names,'Location','NorthWest')
xlabel('execution times [s]')
grid on
mean(execution_times')

114
thirdparty/opengv/matlab/benchmark_relative_pose_noncentral.m vendored Normal file
View File

@@ -0,0 +1,114 @@
%% Reset everything
clear all;
clc;
close all;
addpath('helpers');
%% Configure the benchmark
% noncentral case
cam_number = 4;
% Getting 10 points, and testing all algorithms with the respective number of points
pt_number = 50;
% noise test, so no outliers
outlier_fraction = 0.0;
% repeat 5000 tests per noise level
iterations = 5000;
% The algorithms we want to test
algorithms = { 'seventeenpt'; 'rel_nonlin_noncentral'; 'rel_nonlin_noncentral' };
% This defines the number of points used for every algorithm
indices = { [1:1:17]; [1:1:17]; [1:1:50] };
% The name of the algorithms in the final plots
names = { '17pt'; 'nonlin. opt. (17pts)'; 'nonlin. opt. (50pts)' };
% The maximum noise to analyze
max_noise = 5.0;
% The step in between different noise levels
noise_step = 0.1;
%% Run the benchmark
%prepare the overall result arrays
number_noise_levels = max_noise / noise_step + 1;
num_algorithms = size(algorithms,1);
mean_rotation_errors = zeros(num_algorithms,number_noise_levels);
median_rotation_errors = zeros(num_algorithms,number_noise_levels);
mean_position_errors = zeros(num_algorithms,number_noise_levels);
median_position_errors = zeros(num_algorithms,number_noise_levels);
noise_levels = zeros(1,number_noise_levels);
%Run the experiment
for n=1:number_noise_levels
noise = (n - 1) * noise_step;
noise_levels(1,n) = noise;
display(['Analyzing noise level: ' num2str(noise)])
rotation_errors = zeros(num_algorithms,iterations);
position_errors = zeros(num_algorithms,iterations);
counter = 0;
for i=1:iterations
% generate experiment
[v1,v2,t,R] = create2D2DExperiment(pt_number,cam_number,noise,outlier_fraction);
[t_perturbed,R_perturbed] = perturb(t,R,0.01);
T_perturbed = [R_perturbed,t_perturbed];
T_gt = [R,t];
for a=1:num_algorithms
Out = opengv(algorithms{a},indices{a},v1,v2,T_perturbed);
[position_error, rotation_error] = evaluateTransformationError( T_gt, Out );
position_errors(a,i) = position_error;
rotation_errors(a,i) = rotation_error;
end
counter = counter + 1;
if counter == 100
counter = 0;
display(['Iteration ' num2str(i) ' of ' num2str(iterations) '(noise level ' num2str(noise) ')']);
end
end
%Now compute the mean and median value of the error for each algorithm
for a=1:num_algorithms
mean_rotation_errors(a,n) = mean(rotation_errors(a,:));
median_rotation_errors(a,n) = median(rotation_errors(a,:));
mean_position_errors(a,n) = mean(position_errors(a,:));
median_position_errors(a,n) = median(position_errors(a,:));
end
end
%% Plot the results
figure(1)
plot(noise_levels,mean_rotation_errors,'LineWidth',2)
legend(names,'Location','NorthWest')
xlabel('noise level [pix]')
ylabel('mean rot. error [rad]')
grid on
figure(2)
plot(noise_levels,median_rotation_errors,'LineWidth',2)
legend(names,'Location','NorthWest')
xlabel('noise level [pix]')
ylabel('median rot. error [rad]')
grid on
figure(3)
plot(noise_levels,mean_position_errors,'LineWidth',2)
legend(names,'Location','NorthWest')
xlabel('noise level [pix]')
ylabel('mean pos. error [m]')
grid on
figure(4)
plot(noise_levels,median_position_errors,'LineWidth',2)
legend(names,'Location','NorthWest')
xlabel('noise level [pix]')
ylabel('median pos. error [m]')
grid on

106
thirdparty/opengv/matlab/benchmark_relative_pose_noncentral2.m vendored Normal file
View File

@@ -0,0 +1,106 @@
%% Reset everything
clear all;
clc;
close all;
addpath('helpers');
%% Configure the benchmark
% noncentral case
cam_number = 4;
% Getting 17 points, and testing all algorithms with the respective number of points
pt_number = 17;
% noise test, so no outliers
outlier_fraction = 0.0;
% repeat 1000 tests per noise level
iterations = 1000;
% The algorithms we want to test
algorithms = { 'sixpt'; 'ge'; 'ge'; 'seventeenpt'; 'rel_nonlin_noncentral' };
% This defines the number of points used for every algorithm
indices = { [1:1:6]; [1:1:8]; [1:1:17]; [1:1:17]; [1:1:17] };
% The name of the algorithms in the final plots
names = { '6pt'; 'ge (8pt)'; 'ge (17pt)'; '17pt'; 'nonlin. opt. (17pt)' };
% The maximum noise to analyze
max_noise = 5.0;
% The step in between different noise levels
noise_step = 0.1;
%% Run the benchmark
%prepare the overall result arrays
number_noise_levels = max_noise / noise_step + 1;
num_algorithms = size(algorithms,1);
mean_rotation_errors = zeros(num_algorithms,number_noise_levels);
median_rotation_errors = zeros(num_algorithms,number_noise_levels);
noise_levels = zeros(1,number_noise_levels);
%Run the experiment
for n=1:number_noise_levels
noise = (n - 1) * noise_step;
noise_levels(1,n) = noise;
display(['Analyzing noise level: ' num2str(noise)])
rotation_errors = zeros(num_algorithms,iterations);
counter = 0;
for i=1:iterations
% generate experiment
[v1,v2,t,R] = create2D2DOmniExperiment(pt_number,cam_number,noise,outlier_fraction);
[t_perturbed,R_perturbed] = perturb(t,R,0.01);
T_perturbed = [R_perturbed,t_perturbed];
T_init = [eye(3),zeros(3,1)];
T_gt = [R,t];
for a=1:num_algorithms
if strcmp(algorithms{a},'ge')
Out = opengv(algorithms{a},indices{a},v1,v2,T_init);
else
Out = opengv(algorithms{a},indices{a},v1,v2,T_perturbed);
end
if a > 3 %if a bigger than 4, we obtain entire transformation, and need to "cut" the rotation
temp = Out(:,1:3);
Out = temp;
end
rotation_error = evaluateRotationError( R, Out );
rotation_errors(a,i) = rotation_error;
end
counter = counter + 1;
if counter == 100
counter = 0;
display(['Iteration ' num2str(i) ' of ' num2str(iterations) '(noise level ' num2str(noise) ')']);
end
end
%Now compute the mean and median value of the error for each algorithm
for a=1:num_algorithms
mean_rotation_errors(a,n) = mean(rotation_errors(a,:));
median_rotation_errors(a,n) = median(rotation_errors(a,:));
end
end
%% Plot the results
figure(1)
plot(noise_levels,mean_rotation_errors,'LineWidth',2)
legend(names,'Location','NorthWest')
xlabel('noise level [pix]')
ylabel('mean rot. error [rad]')
grid on
figure(2)
plot(noise_levels,median_rotation_errors,'LineWidth',2)
legend(names,'Location','NorthWest')
xlabel('noise level [pix]')
ylabel('median rot. error [rad]')
grid on

63
thirdparty/opengv/matlab/benchmark_relative_pose_noncentral_execution_times.m vendored Normal file
View File

@@ -0,0 +1,63 @@
%% Reset everything
clear all;
clc;
close all;
addpath('helpers');
%% Configure the benchmark
% central case -> only one camera
cam_number = 4;
% Getting 10 points, and testing all algorithms with the respective number of points
pt_number = 50;
% noise test, so no outliers
outlier_fraction = 0.0;
% repeat 1000 iterations
iterations = 1000;
% The algorithms we want to test
algorithms = { 'seventeenpt' };
% This defines the number of points used for every algorithm
indices = { [1:1:17] };
% The name of the algorithms in the final plots
names = { '17pt' };
% The noise in this experiment
noise = 1.0;
%% Run the benchmark
%prepare the overall result arrays
num_algorithms = size(algorithms,1);
execution_times = zeros(num_algorithms,iterations);
counter = 0;
for i=1:iterations
% generate experiment
[v1,v2,t,R] = create2D2DExperiment(pt_number,cam_number,noise,outlier_fraction);
[t_perturbed,R_perturbed] = perturb(t,R,0.01);
T_perturbed = [R_perturbed,t_perturbed];
for a=1:num_algorithms
tic
Out = opengv_donotuse(algorithms{a},indices{a},v1,v2,T_perturbed);
execution_times(a,i) = toc/20.0;
end
counter = counter + 1;
if counter == 100
counter = 0;
display(['Iteration ' num2str(i) ' of ' num2str(iterations) '(noise level ' num2str(noise) ')']);
end
end
%% Plot the results
hist(execution_times')
legend(names,'Location','NorthWest')
xlabel('execution times [s]')
grid on
mean(execution_times')

91
thirdparty/opengv/matlab/benchmark_relative_pose_noncentral_execution_times2.m vendored Normal file
View File

@@ -0,0 +1,91 @@
%% Reset everything
clear all;
clc;
close all;
addpath('helpers');
%% Configure the benchmark
% central case -> only one camera
cam_number = 4;
% Getting 17 points, and testing all algorithms with the respective number of points
pt_number = 17;
% noise test, so no outliers
outlier_fraction = 0.0;
% repeat 1000 iterations
iterations = 1000;
% The algorithms we want to test
algorithms = { 'sixpt'; 'ge'; 'seventeenpt' };
% This defines the number of points used for every algorithm
indices = { [1:1:6]; [1:1:8]; [1:1:17] };
% The name of the algorithms in the final plots
names = { '6pt';'ge (8pt)'; '17pt' };
% The noise in this experiment
noise = 0.5;
%% Run the benchmark
%prepare the overall result arrays
num_algorithms = size(algorithms,1);
execution_times = zeros(num_algorithms,iterations);
counter = 0;
for i=1:iterations
% generate experiment
[v1,v2,t,R] = create2D2DOmniExperiment(pt_number,cam_number,noise,outlier_fraction);
[t_perturbed,R_perturbed] = perturb(t,R,0.01);
T_perturbed = [R_perturbed,t_perturbed];
T_init = [eye(3) zeros(3,1)];
for a=1:num_algorithms
tic
Out = opengv_donotuse(algorithms{a},indices{a},v1,v2,T_init);
execution_times(a,i) = toc/20.0;
end
counter = counter + 1;
if counter == 1
counter = 0;
display(['Iteration ' num2str(i) ' of ' num2str(iterations) '(noise level ' num2str(noise) ')']);
end
end
%% Plot results
hist(log10(execution_times)')
legend(names,'Location','NorthWest')
xlabel('execution time [s]')
ylabel('occurence')
grid on
%% print the mean and median execution time on the console
display( 'mean execution times:' )
display(['sixpt: ' num2str(mean(execution_times(1,:)'))] );
display(['ge: ' num2str(mean(execution_times(2,:)'))] );
display(['seventeenpt: ' num2str(mean(execution_times(3,:)'))] );
%% Plot the results
%
% [y1,x1] = hist(execution_times(1,:));
% [y2,x2] = hist(execution_times(2,:));
% [y3,x3] = hist(execution_times(3,:));
%
% y1 = y1 / (x1(1,2) - x1(1,1));
% y2 = y2 / (x2(1,2) - x2(1,1));
% y3 = y3 / (x3(1,2) - x3(1,1));
%
% figure(2)
% hold on
% plot(x1,y1,'b');
% plot(x2,y2,'g');
% plot(x3,y3,'r');
% legend(names,'Location','NorthWest')
% xlabel('execution time [s]')
% ylabel('probability')
% grid on

35
thirdparty/opengv/matlab/helpers/addNoise.m vendored Normal file
View File

@@ -0,0 +1,35 @@
function v_noisy = addNoise(v_clean,focal_length,pixel_noise)
%find good vector in normal plane based on good conditioning
inPlaneVector1 = zeros(3,1);
if v_clean(1,1) > v_clean(2,1) && v_clean(1,1) > v_clean(3,1)
inPlaneVector1(2,1) = 1.0;
inPlaneVector1(3,1) = 0.0;
inPlaneVector1(1,1) = 1.0/v_clean(1,1) * (-v_clean(2,1));
else
if v_clean(2,1) > v_clean(3,1) && v_clean(2,1) > v_clean(1,1)
inPlaneVector1(3,1) = 1.0;
inPlaneVector1(1,1) = 0.0;
inPlaneVector1(2,1) = 1.0/v_clean(2,1) * (-v_clean(3,1));
else
%v_clean(3,1) > v_clean(1,1) && v_clean(3,1) > v_clean(2,1)
inPlaneVector1(1,1) = 1.0;
inPlaneVector1(2,1) = 0.0;
inPlaneVector1(3,1) = 1.0/v_clean(3,1) * (-v_clean(1,1));
end
end
%normalize the in-plane vector
inPlaneVector1 = inPlaneVector1 / norm(inPlaneVector1);
inPlaneVector2 = cross(v_clean,inPlaneVector1);
noiseX = pixel_noise * (rand-0.5)*2.0;% / sqrt(2);
noiseY = pixel_noise * (rand-0.5)*2.0;% / sqrt(2);
v_noisy = focal_length * v_clean + noiseX * inPlaneVector1 + noiseY * inPlaneVector2;
v_noisy_norm = norm(v_noisy);
v_noisy = v_noisy ./ v_noisy_norm;
end

20
thirdparty/opengv/matlab/helpers/cayley2rot.m vendored Normal file
View File

@@ -0,0 +1,20 @@
function R = cayley2rot(v)
cayley0 = v(1,1);
cayley1 = v(2,1);
cayley2 = v(3,1);
R = zeros(3,3);
scale = 1+cayley0^2+cayley1^2+cayley2^2;
R(1,1) = 1+cayley0^2-cayley1^2-cayley2^2;
R(1,2) = 2*(cayley0*cayley1-cayley2);
R(1,3) = 2*(cayley0*cayley2+cayley1);
R(2,1) = 2*(cayley0*cayley1+cayley2);
R(2,2) = 1-cayley0^2+cayley1^2-cayley2^2;
R(2,3) = 2*(cayley1*cayley2-cayley0);
R(3,1) = 2*(cayley0*cayley2-cayley1);
R(3,2) = 2*(cayley1*cayley2+cayley0);
R(3,3) = 1-cayley0^2-cayley1^2+cayley2^2;
R = (1/scale) * R;

128
thirdparty/opengv/matlab/helpers/create2D2DExperiment.m vendored Normal file
View File

@@ -0,0 +1,128 @@
function [v1, v2, t, R ] = create2D2DExperiment( pt_number, cam_number, noise, outlier_fraction )
%% generate the camera system
avg_cam_distance = 0.5;
cam_offsets = zeros(3,cam_number);
%cam_rotations = zeros(3,cam_number*3);
if cam_number == 1
cam_offsets = zeros(3,1);
%cam_rotations = eye(3);
else
for i=1:cam_number
cam_offsets(:,i) = avg_cam_distance * generateRandomR() * [1.0; 0.0; 0.0];
%cam_rotations(:,(i-1)*3+1:(i-1)*3+3) = generateRandomR();
end
end
%% generate random view-points
max_parallax = 2.0;
max_rotation = 0.5;
position1 = zeros(3,1);
rotation1 = eye(3);
position2 = max_parallax * 2.0 * (rand(3,1) - repmat(0.5,3,1));
rotation2 = generateBoundedR(max_rotation);
%% Generate random point-cloud
minDepth = 4.0;
maxDepth = 8.0;
normalizedPoints = 2.0*(rand(3,pt_number)-repmat(0.5,3,pt_number));
norms = sqrt(sum(normalizedPoints.*normalizedPoints));
directions = normalizedPoints./repmat(norms,3,1);
points = (maxDepth-minDepth) * normalizedPoints + minDepth * directions;
%% Now create the correspondences by looping through the cameras
focal_length = 800.0;
v1 = zeros(6,pt_number);
v2 = zeros(6,pt_number);
cam_correspondence = 1;
cam_correspondences = zeros(1,pt_number);
for i=1:pt_number
cam_offset = cam_offsets(:,cam_correspondence);
%cam_rotation = cam_rotations(:,(cam_correspondence-1)*3+1:(cam_correspondence-1)*3+3);
body_point1 = rotation1' * (points(:,i)-position1);
body_point2 = rotation2' * (points(:,i)-position2);
% we actually omit the can rotation here by unrotating the bearing
% vectors already
bearingVector1 = body_point1 - cam_offset;
bearingVector2 = body_point2 - cam_offset;
bearingVector1_norm = norm(bearingVector1);
bearingVector2_norm = norm(bearingVector2);
bearingVector1 = bearingVector1/bearingVector1_norm;
bearingVector2 = bearingVector2/bearingVector2_norm;
% add noise to the bearing vectors here
bearingVector1_noisy = addNoise(bearingVector1,focal_length,noise);
bearingVector2_noisy = addNoise(bearingVector2,focal_length,noise);
% store the normalized bearing vectors along with the cameras they are
% being seen (we create correspondences that always originate from the
% same camera, you can change this if you want)
bearingVector1_norm = norm(bearingVector1_noisy);
bearingVector2_norm = norm(bearingVector2_noisy);
v1(:,i) = [bearingVector1_noisy./bearingVector1_norm; cam_offset];
v2(:,i) = [bearingVector2_noisy./bearingVector2_norm; cam_offset];
% change the camera correspondence
cam_correspondences(1,i) = cam_correspondence;
cam_correspondence = cam_correspondence + 1;
if cam_correspondence > cam_number
cam_correspondence = 1;
end
end
%% Add outliers
number_outliers = floor(outlier_fraction*pt_number);
if number_outliers > 0
for i=1:number_outliers
cam_correspondence = cam_correspondences(1,i);
cam_offset = cam_offsets(:,cam_correspondence);
%cam_rotation = cam_rotations(:,(cam_correspondence-1)*3+1:(cam_correspondence-1)*3+3);
%generate random point
normalizedPoint = 2.0*(rand(3,1)-repmat(0.5,3,1));
norm1 = sqrt(sum(normalizedPoint.*normalizedPoint));
direction = normalizedPoint./norm1;
point = (maxDepth-minDepth) * normalizedPoint + minDepth * direction;
body_point2 = rotation2' * (point-position2);
% store the point (no need to add noise)
bearingVector2 = body_point2 - cam_offset;
% store the normalized bearing vectors along with the cameras they are
% being seen (we create correspondences that always originate from the
% same camera, you can change this if you want)
bearingVector2_norm = norm(bearingVector2);
v2(:,i) = [bearingVector2./bearingVector2_norm; cam_offset];
end
end
%% compute relative translation and rotation
R = rotation1' * rotation2;
t = rotation1' * (position2 - position1);
%% Cut the cam offset in the single camera case (e.g. central)
if cam_number == 1
v1 = v1(1:3,:);
v2 = v2(1:3,:);
end

118
thirdparty/opengv/matlab/helpers/create2D2DOmniExperiment.m vendored Normal file
View File

@@ -0,0 +1,118 @@
function [v1, v2, t, R ] = create2D2DOmniExperiment( pt_number, cam_number, noise, outlier_fraction )
%% generate the camera system
cam_distance = 1.0;
%% set a regular camera system with 2 or 4 cameras here
if cam_number == 2
cam_offsets = [ cam_distance -cam_distance; 0.0 0.0; 0.0 0.0 ];
else
cam_number = 4; % only two or 4 supported for this experiment
cam_offsets = [ cam_distance 0.0 -cam_distance 0.0; 0.0 cam_distance 0.0 -cam_distance; 0.0 0.0 0.0 0.0 ];
end
%% generate random view-points
max_parallax = 2.0;
max_rotation = 0.5;
position1 = zeros(3,1);
rotation1 = eye(3);
position2 = max_parallax * 2.0 * (rand(3,1) - repmat(0.5,3,1));
rotation2 = generateBoundedR(max_rotation);
%% Generate random point-cloud
avg_depth_over_cam_distance = 10.0;
maxDepth = 5.0;
normalizedPoints = 2.0*(rand(3,pt_number)-repmat(0.5,3,pt_number));
points = maxDepth * normalizedPoints;
%% Now create the correspondences by looping through the cameras
focal_length = 800.0;
v1 = zeros(6,pt_number);
v2 = zeros(6,pt_number);
cam_correspondence = 1;
cam_correspondences = zeros(1,pt_number);
for i=1:pt_number
cam_offset = cam_offsets(:,cam_correspondence);
%cam_rotation = cam_rotations(:,(cam_correspondence-1)*3+1:(cam_correspondence-1)*3+3);
%special: shift the point in the first frame along current camera axis, which guarantees homogeneous distribution
temp = points(:,i) + avg_depth_over_cam_distance * cam_offset;
points(:,i) = temp;
body_point1 = rotation1' * (points(:,i)-position1);
body_point2 = rotation2' * (points(:,i)-position2);
% we actually omit the can rotation here by unrotating the bearing
% vectors already
bearingVector1 = body_point1 - cam_offset;
bearingVector2 = body_point2 - cam_offset;
bearingVector1_norm = norm(bearingVector1);
bearingVector2_norm = norm(bearingVector2);
bearingVector1 = bearingVector1/bearingVector1_norm;
bearingVector2 = bearingVector2/bearingVector2_norm;
% add noise to the bearing vectors here
bearingVector1_noisy = addNoise(bearingVector1,focal_length,noise);
bearingVector2_noisy = addNoise(bearingVector2,focal_length,noise);
% store the normalized bearing vectors along with the cameras they are
% being seen (we create correspondences that always originate from the
% same camera, you can change this if you want)
bearingVector1_norm = norm(bearingVector1_noisy);
bearingVector2_norm = norm(bearingVector2_noisy);
v1(:,i) = [bearingVector1_noisy./bearingVector1_norm; cam_offset];
v2(:,i) = [bearingVector2_noisy./bearingVector2_norm; cam_offset];
% change the camera correspondence
cam_correspondences(1,i) = cam_correspondence;
cam_correspondence = cam_correspondence + 1;
if cam_correspondence > cam_number
cam_correspondence = 1;
end
end
%% Add outliers
number_outliers = floor(outlier_fraction*pt_number);
if number_outliers > 0
for i=1:number_outliers
cam_correspondence = cam_correspondences(1,i);
cam_offset = cam_offsets(:,cam_correspondence);
%cam_rotation = cam_rotations(:,(cam_correspondence-1)*3+1:(cam_correspondence-1)*3+3);
%generate random point
normalizedPoint = 2.0*(rand(3,1)-repmat(0.5,3,1));
point = maxDepth * normalizedPoint + avg_depth_over_cam_distance * cam_offset;
body_point2 = rotation2' * (point-position2);
% store the point (no need to add noise)
bearingVector2 = body_point2 - cam_offset;
% store the normalized bearing vectors along with the cameras they are
% being seen (we create correspondences that always originate from the
% same camera, you can change this if you want)
bearingVector2_norm = norm(bearingVector2);
v2(:,i) = [bearingVector2./bearingVector2_norm; cam_offset];
end
end
%% compute relative translation and rotation
R = rotation1' * rotation2;
t = rotation1' * (position2 - position1);

116
thirdparty/opengv/matlab/helpers/create2D3DExperiment.m vendored Normal file
View File

@@ -0,0 +1,116 @@
function [points, v, t, R ] = create2D3DExperiment( pt_number, cam_number, noise, outlier_fraction )
%% generate the camera system
avg_cam_distance = 0.5;
cam_offsets = zeros(3,cam_number);
%cam_rotations = zeros(3,cam_number*3);
if cam_number == 1
cam_offsets = zeros(3,1);
%cam_rotations = eye(3);
else
for i=1:cam_number
cam_offsets(:,i) = avg_cam_distance * generateRandomR() * [1.0; 0.0; 0.0];
%cam_rotations(:,(i-1)*3+1:(i-1)*3+3) = generateRandomR();
end
end
%% generate random view-points
max_parallax = 2.0;
max_rotation = 0.5;
position = max_parallax * 2.0 * (rand(3,1) - repmat(0.5,3,1));
rotation = generateBoundedR(max_rotation);
%% Generate random point-cloud
minDepth = 4.0;
maxDepth = 8.0;
normalizedPoints = 2.0*(rand(3,pt_number)-repmat(0.5,3,pt_number));
norms = sqrt(sum(normalizedPoints.*normalizedPoints));
directions = normalizedPoints./repmat(norms,3,1);
points = (maxDepth-minDepth) * normalizedPoints + minDepth * directions;
%% Now create the correspondences by looping through the cameras
focal_length = 800.0;
v = zeros(6,pt_number);
cam_correspondence = 1;
cam_correspondences = zeros(1,pt_number);
for i=1:pt_number
cam_offset = cam_offsets(:,cam_correspondence);
%cam_rotation = cam_rotations(:,(cam_correspondence-1)*3+1:(cam_correspondence-1)*3+3);
body_point = rotation' * (points(:,i)-position);
% we actually omit the can rotation here by unrotating the bearing
% vectors already
bearingVector = body_point - cam_offset;
bearingVector_norm = norm(bearingVector);
bearingVector = bearingVector/bearingVector_norm;
% add noise to the bearing vectors here
bearingVector_noisy = addNoise(bearingVector,focal_length,noise);
% store the normalized bearing vectors along with the cameras they are
% being seen (we create correspondences that always originate from the
% same camera, you can change this if you want)
bearingVector_norm = norm(bearingVector_noisy);
v(:,i) = [bearingVector_noisy./bearingVector_norm; cam_offset];
% change the camera correspondence
cam_correspondences(1,i) = cam_correspondence;
cam_correspondence = cam_correspondence + 1;
if cam_correspondence > cam_number
cam_correspondence = 1;
end
end
%% Add outliers
number_outliers = floor(outlier_fraction*pt_number);
if number_outliers > 0
for i=1:number_outliers
cam_correspondence = cam_correspondences(1,i);
cam_offset = cam_offsets(:,cam_correspondence);
%cam_rotation = cam_rotations(:,(cam_correspondence-1)*3+1:(cam_correspondence-1)*3+3);
%generate random point
normalizedPoint = 2.0*(rand(3,1)-repmat(0.5,3,1));
norm1 = sqrt(sum(normalizedPoint.*normalizedPoint));
direction = normalizedPoint./norm1;
point = (maxDepth-minDepth) * normalizedPoint + minDepth * direction;
body_point = rotation' * (point-position);
% store the point (no need to add noise)
bearingVector = body_point - cam_offset;
% store the normalized bearing vectors along with the cameras they are
% being seen (we create correspondences that always originate from the
% same camera, you can change this if you want)
bearingVector_norm = norm(bearingVector);
v(:,i) = [bearingVector./bearingVector_norm; cam_offset];
end
end
%% copy over the position and orientation
t = position;
R = rotation;
%% cut the cam offsets in the single camera (e.g. central case)
if cam_number == 1
v = v(1:3,:);
end

119
thirdparty/opengv/matlab/helpers/createMulti2D2DExperiment.m vendored Normal file
View File

@@ -0,0 +1,119 @@
function [v1, v2, cam_offsets, t, R ] = createMulti2D2DExperiment( pt_per_cam, cam_number, noise, outlier_fraction )
%% generate the camera system
avg_cam_distance = 0.5;
cam_offsets = zeros(3,cam_number);
%cam_rotations = zeros(3,cam_number*3);
if cam_number == 1
cam_offsets = zeros(3,1);
%cam_rotations = eye(3);
else
for i=1:cam_number
cam_offsets(:,i) = avg_cam_distance * generateRandomR() * [1.0; 0.0; 0.0];
%cam_rotations(:,(i-1)*3+1:(i-1)*3+3) = generateRandomR();
end
end
%% generate random view-points
max_parallax = 2.0;
max_rotation = 0.5;
position1 = zeros(3,1);
rotation1 = eye(3);
position2 = max_parallax * 2.0 * (rand(3,1) - repmat(0.5,3,1));
rotation2 = generateBoundedR(max_rotation);
%% Generate random point-clouds
minDepth = 4.0;
maxDepth = 8.0;
p = cell([cam_number 1]);
for cam=1:cam_number
normalizedPoints = 2.0*(rand(3,pt_per_cam)-repmat(0.5,3,pt_per_cam));
norms = sqrt(sum(normalizedPoints.*normalizedPoints));
directions = normalizedPoints./repmat(norms,3,1);
p{cam,1} = (maxDepth-minDepth) * normalizedPoints + minDepth * directions;
end
%% Now create the correspondences by looping through the cameras
focal_length = 800.0;
v1 = cell([cam_number 1]);
v2 = cell([cam_number 1]);
for cam=1:cam_number
v1{cam,1} = zeros(3,pt_per_cam);
v2{cam,1} = zeros(3,pt_per_cam);
for i=1:pt_per_cam
cam_offset = cam_offsets(:,cam);
%cam_rotation = cam_rotations(:,(cam-1)*3+1:(cam-1)*3+3);
body_point1 = rotation1' * (p{cam,1}(:,i)-position1);
body_point2 = rotation2' * (p{cam,1}(:,i)-position2);
% we actually omit the cam rotation here by unrotating the bearing
% vectors already
bearingVector1 = body_point1 - cam_offset;
bearingVector2 = body_point2 - cam_offset;
bearingVector1_norm = norm(bearingVector1);
bearingVector2_norm = norm(bearingVector2);
bearingVector1 = bearingVector1/bearingVector1_norm;
bearingVector2 = bearingVector2/bearingVector2_norm;
% add noise to the bearing vectors here
bearingVector1_noisy = addNoise(bearingVector1,focal_length,noise);
bearingVector2_noisy = addNoise(bearingVector2,focal_length,noise);
% store the normalized bearing vectors along with the cameras they are
% being seen (we create correspondences that always originate from the
% same camera, you should not change this in this experiment!)
bearingVector1_norm = norm(bearingVector1_noisy);
bearingVector2_norm = norm(bearingVector2_noisy);
v1{cam,1}(:,i) = bearingVector1_noisy./bearingVector1_norm;
v2{cam,1}(:,i) = bearingVector2_noisy./bearingVector2_norm;
end
end
%% Add outliers
outliers_per_cam = floor(outlier_fraction*pt_per_cam);
if outliers_per_cam > 0
for cam=1:cam_number
for i=1:outliers_per_cam
cam_offset = cam_offsets(:,cam);
%cam_rotation = cam_rotations(:,(cam-1)*3+1:(cam-1)*3+3);
%generate random point
normalizedPoint = 2.0*(rand(3,1)-repmat(0.5,3,1));
norm1 = sqrt(sum(normalizedPoint.*normalizedPoint));
direction = normalizedPoint./norm1;
point = (maxDepth-minDepth) * normalizedPoint + minDepth * direction;
body_point2 = rotation2' * (point-position2);
% store the point (no need to add noise)
bearingVector2 = body_point2 - cam_offset;
bearingVector2_norm = norm(bearingVector2);
v2{cam,1}(:,i) = bearingVector2./bearingVector2_norm;
end
end
end
%% compute relative translation and rotation
R = rotation1' * rotation2;
t = rotation1' * (position2 - position1);

120
thirdparty/opengv/matlab/helpers/createMulti2D2DOmniExperiment.m vendored Normal file
View File

@@ -0,0 +1,120 @@
function [v1, v2, cam_offsets, t, R ] = createMulti2D2DOmniExperiment( pt_per_cam, cam_number, noise, outlier_fraction )
%% generate the camera system
cam_distance = 1.0;
%% set a regular camera system with 2 or 4 cameras here
if cam_number == 2
cam_offsets = [ cam_distance -cam_distance; 0.0 0.0; 0.0 0.0 ];
else
cam_number = 4; % only two or 4 supported for this experiment
cam_offsets = [ cam_distance 0.0 -cam_distance 0.0; 0.0 cam_distance 0.0 -cam_distance; 0.0 0.0 0.0 0.0 ];
end
%% generate random view-points
max_parallax = 2.0;
max_rotation = 0.5;
position1 = zeros(3,1);
rotation1 = eye(3);
position2 = max_parallax * 2.0 * (rand(3,1) - repmat(0.5,3,1));
rotation2 = generateBoundedR(max_rotation);
%% Generate random point-cloud
avg_depth_over_cam_distance = 10.0;
maxDepth = 5.0;
p = cell([cam_number 1]);
for cam=1:cam_number
normalizedPoints = 2.0*(rand(3,pt_per_cam)-repmat(0.5,3,pt_per_cam));
p{cam,1} = maxDepth * normalizedPoints;
end
%% Now create the correspondences by looping through the cameras
focal_length = 800.0;
v1 = cell([cam_number 1]);
v2 = cell([cam_number 1]);
for cam=1:cam_number
v1{cam,1} = zeros(3,pt_per_cam);
v2{cam,1} = zeros(3,pt_per_cam);
for i=1:pt_per_cam
cam_offset = cam_offsets(:,cam);
%special: shift the point in the first frame along current camera axis, which guarantees homogeneous distribution
temp = p{cam,1}(:,i) + avg_depth_over_cam_distance * cam_offset;
p{cam,1}(:,i) = temp;
body_point1 = rotation1' * (p{cam,1}(:,i)-position1);
body_point2 = rotation2' * (p{cam,1}(:,i)-position2);
% we actually omit the can rotation here by unrotating the bearing
% vectors already
bearingVector1 = body_point1 - cam_offset;
bearingVector2 = body_point2 - cam_offset;
bearingVector1_norm = norm(bearingVector1);
bearingVector2_norm = norm(bearingVector2);
bearingVector1 = bearingVector1/bearingVector1_norm;
bearingVector2 = bearingVector2/bearingVector2_norm;
% add noise to the bearing vectors here
bearingVector1_noisy = addNoise(bearingVector1,focal_length,noise);
bearingVector2_noisy = addNoise(bearingVector2,focal_length,noise);
% store the normalized bearing vectors along with the cameras they are
% being seen (we create correspondences that always originate from the
% same camera, you can change this if you want)
bearingVector1_norm = norm(bearingVector1_noisy);
bearingVector2_norm = norm(bearingVector2_noisy);
v1{cam,1}(:,i) = [bearingVector1_noisy./bearingVector1_norm];
v2{cam,1}(:,i) = [bearingVector2_noisy./bearingVector2_norm];
end
end
%% Add outliers
outliers_per_cam = floor(outlier_fraction*pt_per_cam);
if outliers_per_cam > 0
for cam=1:cam_number
for i=1:outliers_per_cam
cam_offset = cam_offsets(:,cam);
%generate random point
normalizedPoint = 2.0*(rand(3,1)-repmat(0.5,3,1));
point = maxDepth * normalizedPoint + avg_depth_over_cam_distance * cam_offset;
body_point2 = rotation2' * (point-position2);
% store the point (no need to add noise)
bearingVector2 = body_point2 - cam_offset;
% store the normalized bearing vectors along with the cameras they are
% being seen (we create correspondences that always originate from the
% same camera, you can change this if you want)
bearingVector2_norm = norm(bearingVector2);
v2{cam,1}(:,i) = [bearingVector2./bearingVector2_norm];
end
end
end
%% compute relative translation and rotation
R = rotation1' * rotation2;
t = rotation1' * (position2 - position1);

38
thirdparty/opengv/matlab/helpers/evaluateRotationError.m vendored Normal file
View File

@@ -0,0 +1,38 @@
function rotation_error = evaluateRotationError(R_gt,R)
temp = size(size(R));
numberSolutions = 1;
if temp(1,2) == 3
temp2 = size(R);
numberSolutions = temp2(1,3);
end
if numberSolutions == 1
%rotation_error = norm(rodrigues(R_gt'*R));
rotation_error = norm( rodrigues(R_gt) - rodrigues(R) );
else
rotation_errors = zeros(1,numberSolutions);
index = 0;
for i=1:numberSolutions
%Check if there is any Nan
if ~isnan(R(1,1,i))
index = index + 1;
%rotation_errors(1,index) = norm(rodrigues(R_gt'*R(:,:,i)));
rotation_errors(1,index) = norm( rodrigues(R_gt) - rodrigues(R(:,:,i)) );
end
end
%find the smallest error (we are the most "nice" to algorithms returning multiple solutions,
%and do the disambiguation by hand)
[~,minIndex] = min(rotation_errors(1,1:index));
rotation_error = rotation_errors(1,minIndex);
end
end

40
thirdparty/opengv/matlab/helpers/evaluateTransformationError.m vendored Normal file
View File

@@ -0,0 +1,40 @@
function [position_error,rotation_error] = evaluateTransformationError(T_gt,T)
temp = size(size(T));
numberSolutions = 1;
if temp(1,2) == 3
temp2 = size(T);
numberSolutions = temp2(1,3);
end
if numberSolutions == 1
position_error = norm(T_gt(:,4)-T(:,4));
rotation_error = norm(rodrigues(T_gt(:,1:3)'*T(:,1:3)));
else
position_errors = zeros(1,numberSolutions);
rotation_errors = zeros(1,numberSolutions);
index = 0;
for i=1:numberSolutions
%Check if there is any Nan
if ~isnan(T(1,1,i))
index = index + 1;
position_errors(1,index) = norm(T_gt(:,4)-T(:,4,i));
rotation_errors(1,index) = norm(rodrigues(T_gt(:,1:3)'*T(:,1:3,i)));
end
end
%find the smallest error (we are the most "nice" to algorithms returning multiple solutions,
%and do the disambiguation by hand)
[~,minIndex] = min(position_errors(1,1:index));
position_error = position_errors(1,minIndex);
rotation_error = rotation_errors(1,minIndex);
end
end

26
thirdparty/opengv/matlab/helpers/generateBoundedR.m vendored Normal file
View File

@@ -0,0 +1,26 @@
function R = generateBoundedR( bound )
rpy = bound*2.0*(rand(3,1)-repmat(0.5,3,1));
R1 = zeros(3,3);
R1(1,1) = 1.0;
R1(2,2) = cos(rpy(1,1));
R1(2,3) = -sin(rpy(1,1));
R1(3,2) = -R1(2,3);
R1(3,3) = R1(2,2);
R2 = zeros(3,3);
R2(1,1) = cos(rpy(2,1));
R2(1,3) = sin(rpy(2,1));
R2(2,2) = 1.0;
R2(3,1) = -R2(1,3);
R2(3,3) = R2(1,1);
R3 = zeros(3,3);
R3(1,1) = cos(rpy(3,1));
R3(1,2) = -sin(rpy(3,1));
R3(2,1) =-R3(1,2);
R3(2,2) = R3(1,1);
R3(3,3) = 1.0;
R = R3 * R2 * R1;
end

27
thirdparty/opengv/matlab/helpers/generateRandomR.m vendored Normal file
View File

@@ -0,0 +1,27 @@
function R = generateRandomR()
rpy = pi()*2.0*(rand(3,1)-repmat(0.5,3,1));
rpy(2,1) = 0.5*rpy(2,1);
R1 = zeros(3,3);
R1(1,1) = 1.0;
R1(2,2) = cos(rpy(1,1));
R1(2,3) = -sin(rpy(1,1));
R1(3,2) = -R1(2,3);
R1(3,3) = R1(2,2);
R2 = zeros(3,3);
R2(1,1) = cos(rpy(2,1));
R2(1,3) = sin(rpy(2,1));
R2(2,2) = 1.0;
R2(3,1) = -R2(1,3);
R2(3,3) = R2(1,1);
R3 = zeros(3,3);
R3(1,1) = cos(rpy(3,1));
R3(1,2) = -sin(rpy(3,1));
R3(2,1) =-R3(1,2);
R3(2,2) = R3(1,1);
R3(3,3) = 1.0;
R = R3 * R2 * R1;
end

12
thirdparty/opengv/matlab/helpers/perturb.m vendored Normal file
View File

@@ -0,0 +1,12 @@
function [t_perturbed,R_perturbed] = perturb(t,R,amplitude)
t_perturbed = t;
r = rodrigues(R);
for i=1:3
t_perturbed(i,1) = t_perturbed(i,1) + (rand-0.5)*2.0*amplitude;
r(i,1) = r(i,1) + (rand-0.5)*2.0*amplitude;
end
R_perturbed = rodrigues(r);
end

220
thirdparty/opengv/matlab/helpers/rodrigues.m vendored Normal file
View File

@@ -0,0 +1,220 @@
function [out,dout]=rodrigues(in)
% RODRIGUES Transform rotation matrix into rotation vector and viceversa.
%
% Sintax: [OUT]=RODRIGUES(IN)
% If IN is a 3x3 rotation matrix then OUT is the
% corresponding 3x1 rotation vector
% if IN is a rotation 3-vector then OUT is the
% corresponding 3x3 rotation matrix
%
%%
%% Copyright (c) March 1993 -- Pietro Perona
%% California Institute of Technology
%%
%% ALL CHECKED BY JEAN-YVES BOUGUET, October 1995.
%% FOR ALL JACOBIAN MATRICES !!! LOOK AT THE TEST AT THE END !!
%% BUG when norm(om)=pi fixed -- April 6th, 1997;
%% Jean-Yves Bouguet
%% Add projection of the 3x3 matrix onto the set of special ortogonal matrices SO(3) by SVD -- February 7th, 2003;
%% Jean-Yves Bouguet
[m,n] = size(in);
%bigeps = 10e+4*eps;
bigeps = 10e+20*eps;
if ((m==1) & (n==3)) | ((m==3) & (n==1)) %% it is a rotation vector
theta = norm(in);
if theta < eps
R = eye(3);
%if nargout > 1,
dRdin = [0 0 0;
0 0 1;
0 -1 0;
0 0 -1;
0 0 0;
1 0 0;
0 1 0;
-1 0 0;
0 0 0];
%end;
else
if n==length(in) in=in'; end; %% make it a column vec. if necess.
%m3 = [in,theta]
dm3din = [eye(3);in'/theta];
omega = in/theta;
%m2 = [omega;theta]
dm2dm3 = [eye(3)/theta -in/theta^2; zeros(1,3) 1];
alpha = cos(theta);
beta = sin(theta);
gamma = 1-cos(theta);
omegav=[[0 -omega(3) omega(2)];[omega(3) 0 -omega(1)];[-omega(2) omega(1) 0 ]];
A = omega*omega';
%m1 = [alpha;beta;gamma;omegav;A];
dm1dm2 = zeros(21,4);
dm1dm2(1,4) = -sin(theta);
dm1dm2(2,4) = cos(theta);
dm1dm2(3,4) = sin(theta);
dm1dm2(4:12,1:3) = [0 0 0 0 0 1 0 -1 0;
0 0 -1 0 0 0 1 0 0;
0 1 0 -1 0 0 0 0 0]';
w1 = omega(1);
w2 = omega(2);
w3 = omega(3);
dm1dm2(13:21,1) = [2*w1;w2;w3;w2;0;0;w3;0;0];
dm1dm2(13: 21,2) = [0;w1;0;w1;2*w2;w3;0;w3;0];
dm1dm2(13:21,3) = [0;0;w1;0;0;w2;w1;w2;2*w3];
R = eye(3)*alpha + omegav*beta + A*gamma;
dRdm1 = zeros(9,21);
dRdm1([1 5 9],1) = ones(3,1);
dRdm1(:,2) = omegav(:);
dRdm1(:,4:12) = beta*eye(9);
dRdm1(:,3) = A(:);
dRdm1(:,13:21) = gamma*eye(9);
dRdin = dRdm1 * dm1dm2 * dm2dm3 * dm3din;
end;
out = R;
dout = dRdin;
%% it is prob. a rot matr.
elseif ((m==n) & (m==3) & (norm(in' * in - eye(3)) < bigeps)...
& (abs(det(in)-1) < bigeps))
R = in;
% project the rotation matrix to SO(3);
[U,S,V] = svd(R);
R = U*V';
tr = (trace(R)-1)/2;
dtrdR = [1 0 0 0 1 0 0 0 1]/2;
theta = real(acos(tr));
if sin(theta) >= 1e-5,
dthetadtr = -1/sqrt(1-tr^2);
dthetadR = dthetadtr * dtrdR;
% var1 = [vth;theta];
vth = 1/(2*sin(theta));
dvthdtheta = -vth*cos(theta)/sin(theta);
dvar1dtheta = [dvthdtheta;1];
dvar1dR = dvar1dtheta * dthetadR;
om1 = [R(3,2)-R(2,3), R(1,3)-R(3,1), R(2,1)-R(1,2)]';
dom1dR = [0 0 0 0 0 1 0 -1 0;
0 0 -1 0 0 0 1 0 0;
0 1 0 -1 0 0 0 0 0];
% var = [om1;vth;theta];
dvardR = [dom1dR;dvar1dR];
% var2 = [om;theta];
om = vth*om1;
domdvar = [vth*eye(3) om1 zeros(3,1)];
dthetadvar = [0 0 0 0 1];
dvar2dvar = [domdvar;dthetadvar];
out = om*theta;
domegadvar2 = [theta*eye(3) om];
dout = domegadvar2 * dvar2dvar * dvardR;
else
if tr > 0; % case norm(om)=0;
out = [0 0 0]';
dout = [0 0 0 0 0 1/2 0 -1/2 0;
0 0 -1/2 0 0 0 1/2 0 0;
0 1/2 0 -1/2 0 0 0 0 0];
else % case norm(om)=pi; %% fixed April 6th
out = theta * (sqrt((diag(R)+1)/2).*[1;2*(R(1,2:3)>=0)'-1]);
%keyboard;
if nargout > 1,
fprintf(1,'WARNING!!!! Jacobian domdR undefined!!!\n');
dout = NaN*ones(3,9);
end;
end;
end;
else
error('Neither a rotation matrix nor a rotation vector were provided');
end;
return;
%% test of the Jacobians:
%%%% TEST OF dRdom:
om = randn(3,1);
dom = randn(3,1)/1000000;
[R1,dR1] = rodrigues(om);
R2 = rodrigues(om+dom);
R2a = R1 + reshape(dR1 * dom,3,3);
gain = norm(R2 - R1)/norm(R2 - R2a)
%%% TEST OF dOmdR:
om = randn(3,1);
R = rodrigues(om);
dom = randn(3,1)/10000;
dR = rodrigues(om+dom) - R;
[omc,domdR] = rodrigues(R);
[om2] = rodrigues(R+dR);
om_app = omc + domdR*dR(:);
gain = norm(om2 - omc)/norm(om2 - om_app)
%%% OTHER BUG: (FIXED NOW!!!)
omu = randn(3,1);
omu = omu/norm(omu)
om = pi*omu;
[R,dR]= rodrigues(om);
[om2] = rodrigues(R);
[om om2]
%%% NORMAL OPERATION
om = randn(3,1);
[R,dR]= rodrigues(om);
[om2] = rodrigues(R);
[om om2]

12
thirdparty/opengv/matlab/helpers/rot2cayley.m vendored Normal file
View File

@@ -0,0 +1,12 @@
function v = rot2cayley(R)
C1 = R-eye(3);
C2 = R+eye(3);
C = C1 * inv(C2);
v = zeros(3,1);
v(1,1) = -C(2,3);
v(2,1) = C(1,3);
v(3,1) = -C(1,2);
end

53
thirdparty/opengv/matlab/helpers/transformEssentials.m vendored Normal file
View File

@@ -0,0 +1,53 @@
function Rs = transformEssentials(Es)
temp = size(size(Es));
numberSolutions = 1;
if temp(1,2) == 3
temp2 = size(Es);
numberSolutions = temp2(1,3);
end
if numberSolutions == 1
Rs = zeros(3,3,2);
[U,~,V] = svd(Es);
W = [0 -1 0; 1 0 0; 0 0 1];
Rs(1:3,1:3) = U * W * V';
Rs(1:3,4:6) = U * W' * V';
if( det(Rs(1:3,1:3)) < 0 )
Rs(1:3,1:3) = -Rs(1:3,1:3);
end
if( det(Rs(1:3,4:6)) < 0 )
Rs(1:3,4:6) = -Rs(1:3,4:6);
end
else
Rs_temp = zeros(3,3,2*numberSolutions);
index = 0;
for i=1:numberSolutions
%Check if there is any Nan
if ~isnan(Es(1,1,i))
[U,~,V] = svd(Es(:,:,i));
W = [0 -1 0; 1 0 0; 0 0 1];
index = index + 1;
Rs_temp( :,:, index ) = U * W * V';
if(det(Rs_temp( :,:, index )) < 0)
Rs_temp( :,:, index ) = -Rs_temp( :,:, index );
end
index = index + 1;
Rs_temp( :,:, index ) = U * W' * V';
if(det(Rs_temp( :,:, index )) < 0)
Rs_temp( :,:, index ) = -Rs_temp( :,:, index );
end
end
end
Rs = Rs_temp(:,:,1:index);
end
end

21
thirdparty/opengv/matlab/make_mex.sh vendored Executable file
View File

@@ -0,0 +1,21 @@
#!/bin/bash
# Generate MEX files by compiling from Matlab
# This scripts intends to automate the compilation process
# by following the instructions provided in
# http://laurentkneip.github.io/opengv/page_installation.html#sec_installation_5
# We assume the installation configuration is standard
# so that every dependency can be found in an automated way
# We assume a launcher command is available: matlab
# Add OpenGV library directory to the path
export LD_LIBRARY_PATH=../build/lib:$LD_LIBRARY_PATH
# Find path to Eigen library (assumes CMake has cached EIGEN_INCLUDE_DIRS)
# See https://stackoverflow.com/questions/8474753/how-to-get-a-cmake-variable-from-the-command-line
EigenPath=$(cmake -L ../build | grep EIGEN_INCLUDE_DIRS | cut -d "=" -f2)
# Call Matlab with the compilation command
matlab -nodisplay -nosplash -nodesktop \
-r "mex -I../include -I${EigenPath} -L../build/lib -lopengv opengv.cpp -cxx, mex -I../include -I${EigenPath} -L../build/lib -lopengv opengv_donotuse.cpp -cxx, exit"

1725
thirdparty/opengv/matlab/opengv.cpp vendored Normal file
View File

File diff suppressed because it is too large Load Diff

1828
thirdparty/opengv/matlab/opengv_donotuse.cpp vendored Normal file
View File

File diff suppressed because it is too large Load Diff

131
thirdparty/opengv/matlab/opengv_experimental1.cpp vendored Normal file
View File

@@ -0,0 +1,131 @@
static const char *copyright =
" Copyright (c) 2013 Laurent Kneip, ANU. All rights reserved.";
/******************************************************************************
* Author: Laurent Kneip *
* Contact: kneip.laurent@gmail.com *
* License: Copyright (c) 2013 Laurent Kneip, ANU. All rights reserved. *
* *
* Redistribution and use in source and binary forms, with or without *
* modification, are permitted provided that the following conditions *
* are met: *
* * Redistributions of source code must retain the above copyright *
* notice, this list of conditions and the following disclaimer. *
* * Redistributions in binary form must reproduce the above copyright *
* notice, this list of conditions and the following disclaimer in the *
* documentation and/or other materials provided with the distribution. *
* * Neither the name of ANU nor the names of its contributors may be *
* used to endorse or promote products derived from this software without *
* specific prior written permission. *
* *
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"*
* AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE *
* IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE *
* ARE DISCLAIMED. IN NO EVENT SHALL ANU OR THE CONTRIBUTORS BE LIABLE *
* FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL *
* DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR *
* SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER *
* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT *
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY *
* OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF *
* SUCH DAMAGE. *
******************************************************************************/
// Matlab usage:
//
// X = opengv_experimental1( data11, data12, data13, ..., data21, data22, data23, ..., camOffsets, algorithm )
//
// where
// data1x and data2x are matched points (each one of dimension 3xn)
// camOffsets is a 3xn matrix, with being the number of cameras
// algorithm is 0 for sixpt, 1 for ge, and 2 for seventeenpt
// X is a 3x5 matrix returning the found transformation, plus the number of
// Ransac-iterations and inliers
//
//matlab header
//standard headers
#include <stdlib.h>
#include <stdio.h>
#include <vector>
#include "mex.h"
//include generic headers for opengv stuff
#include <opengv/types.hpp>
//include the matlab-adapter
#include <opengv/relative_pose/MANoncentralRelativeMulti.hpp>
//expose all ransac-facilities to matlab
#include <opengv/sac/MultiRansac.hpp>
#include <opengv/sac_problems/relative_pose/MultiNoncentralRelativePoseSacProblem.hpp>
typedef opengv::sac_problems::relative_pose::MultiNoncentralRelativePoseSacProblem nrelRansac;
typedef std::shared_ptr<nrelRansac> nrelRansacPtr;
// The main mex-function
void mexFunction( int nlhs, mxArray *plhs[], int nrhs, const mxArray *prhs[] )
{
//no error-checking here yet, simply provide the right input!!
//get number of cameras
int numberCams = (nrhs-2)/2;
const mxArray *camOffsets = prhs[nrhs-2];
const mxArray *temp1 = prhs[nrhs-1];
double *temp2 = (double*) mxGetData(temp1);
int algorithm = floor(temp2[0]+0.01);
std::vector<double*> bearingVectors1;
std::vector<double*> bearingVectors2;
std::vector<int> numberBearingVectors;
for( int cam = 0; cam < numberCams; cam++ )
{
const mxArray *data1 = prhs[cam];
const mxArray *data2 = prhs[cam+numberCams];
bearingVectors1.push_back((double*) mxGetData(data1));
bearingVectors2.push_back((double*) mxGetData(data2));
const mwSize *dataDim = mxGetDimensions(data1);
numberBearingVectors.push_back(dataDim[1]);
}
opengv::relative_pose::RelativeMultiAdapterBase* relativeAdapter =
new opengv::relative_pose::MANoncentralRelativeMulti(
bearingVectors1,
bearingVectors2,
(double*) mxGetData(camOffsets),
numberBearingVectors );
nrelRansacPtr problem;
switch(algorithm)
{
case 0:
problem = nrelRansacPtr( new nrelRansac( *relativeAdapter, nrelRansac::SIXPT ) );
break;
case 1:
problem = nrelRansacPtr( new nrelRansac( *relativeAdapter, nrelRansac::GE ) );
break;
case 2:
problem = nrelRansacPtr( new nrelRansac( *relativeAdapter, nrelRansac::SEVENTEENPT ) );
break;
}
opengv::sac::MultiRansac<nrelRansac> ransac;
ransac.sac_model_ = problem;
ransac.threshold_ = 2.0*(1.0 - cos(atan(sqrt(2.0)*0.5/800.0)));
ransac.max_iterations_ = 10000000;
ransac.computeModel();
Eigen::Matrix<double,3,5> result;
result.block<3,4>(0,0) = ransac.model_coefficients_;
result(0,4) = ransac.iterations_;
result(1,4) = ransac.inliers_.size();
int dims[2];
dims[0] = 3;
dims[1] = 5;
plhs[0] = mxCreateNumericArray(2, dims, mxDOUBLE_CLASS, mxREAL);
memcpy(mxGetData(plhs[0]), result.data(), 15*sizeof(double));
}

119
thirdparty/opengv/matlab/opengv_experimental2.cpp vendored Normal file
View File

@@ -0,0 +1,119 @@
static const char *copyright =
" Copyright (c) 2013 Laurent Kneip, ANU. All rights reserved.";
/******************************************************************************
* Author: Laurent Kneip *
* Contact: kneip.laurent@gmail.com *
* License: Copyright (c) 2013 Laurent Kneip, ANU. All rights reserved. *
* *
* Redistribution and use in source and binary forms, with or without *
* modification, are permitted provided that the following conditions *
* are met: *
* * Redistributions of source code must retain the above copyright *
* notice, this list of conditions and the following disclaimer. *
* * Redistributions in binary form must reproduce the above copyright *
* notice, this list of conditions and the following disclaimer in the *
* documentation and/or other materials provided with the distribution. *
* * Neither the name of ANU nor the names of its contributors may be *
* used to endorse or promote products derived from this software without *
* specific prior written permission. *
* *
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"*
* AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE *
* IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE *
* ARE DISCLAIMED. IN NO EVENT SHALL ANU OR THE CONTRIBUTORS BE LIABLE *
* FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL *
* DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR *
* SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER *
* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT *
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY *
* OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF *
* SUCH DAMAGE. *
******************************************************************************/
// Matlab usage:
//
// X = opengv_experimental2( data1, data2, algorithm )
//
// where
// data1, data2 are matched points of dimension 6xn
// algorithm is 0 for sixpt, 1 for ge, and 2 for seventeenpt
// X is a 3x5 matrix returning the found transformation, plus the number of
// Ransac-iterations and inliers
//
//matlab header
//standard headers
#include <stdlib.h>
#include <stdio.h>
#include <vector>
#include "mex.h"
//include generic headers for opengv stuff
#include <opengv/types.hpp>
//include the matlab-adapters
#include <opengv/relative_pose/MANoncentralRelative.hpp>
//expose all ransac-facilities to matlab
#include <opengv/sac/Ransac.hpp>
#include <opengv/sac_problems/relative_pose/NoncentralRelativePoseSacProblem.hpp>
typedef opengv::sac_problems::relative_pose::NoncentralRelativePoseSacProblem nrelRansac;
typedef std::shared_ptr<nrelRansac> nrelRansacPtr;
// The main mex-function
void mexFunction( int nlhs, mxArray *plhs[], int nrhs, const mxArray *prhs[] )
{
// Characterize the type of the call
const mxArray *data1 = prhs[0];
const mxArray *data2 = prhs[1];
const mxArray *temp1 = prhs[2];
double *temp2 = (double*) mxGetData(temp1);
int algorithm = floor(temp2[0]+0.01);
const mwSize *data1dim = mxGetDimensions(data1);
const mwSize *data2dim = mxGetDimensions(data2);
//create three pointers to absolute, relative, and point_cloud adapters here
opengv::relative_pose::RelativeAdapterBase* relativeAdapter =
new opengv::relative_pose::MANoncentralRelative(
(double*) mxGetData(data1),
(double*) mxGetData(data2),
data1dim[1],
data2dim[1] );
nrelRansacPtr problem;
switch(algorithm)
{
case 0:
problem = nrelRansacPtr( new nrelRansac( *relativeAdapter, nrelRansac::SIXPT ) );
break;
case 1:
problem = nrelRansacPtr( new nrelRansac( *relativeAdapter, nrelRansac::GE ) );
break;
case 2:
problem = nrelRansacPtr( new nrelRansac( *relativeAdapter, nrelRansac::SEVENTEENPT ) );
break;
}
opengv::sac::Ransac<nrelRansac> ransac;
ransac.sac_model_ = problem;
ransac.threshold_ = 2.0*(1.0 - cos(atan(sqrt(2.0)*0.5/800.0)));
ransac.max_iterations_ = 10000000;
ransac.computeModel();
Eigen::Matrix<double,3,5> result;
result.block<3,4>(0,0) = ransac.model_coefficients_;
result(0,4) = ransac.iterations_;
result(1,4) = ransac.inliers_.size();
int dims[2];
dims[0] = 3;
dims[1] = 5;
plhs[0] = mxCreateNumericArray(2, dims, mxDOUBLE_CLASS, mxREAL);
memcpy(mxGetData(plhs[0]), result.data(), 15*sizeof(double));
}

63
thirdparty/opengv/matlab/plot_arun_error.m vendored Normal file
View File

@@ -0,0 +1,63 @@
%% Reset everything
clear all;
clc;
close all;
addpath('helpers');
%% Configure the benchmark
% noncentral case
cam_number = 4;
% Getting 17 points, and testing all algorithms with the respective number of points
pt_number = 8;
% noise test, so no outliers
outlier_fraction = 0.0;
% repeat 1000 tests per noise level
iterations = 10000;
% The algorithms we want to test
algorithms = { 'ge2' };
% This defines the number of points used for every algorithm
indices = { [1:1:8] };
% The name of the algorithms in the final plots
names = { 'arun (8pt)' };
% The maximum noise to analyze
noise = 0.5;
%% Run the benchmark
%Run the experiment
rotation_errors = zeros(1,iterations);
counter = 0;
for i=1:iterations
% generate experiment
[v1,v2,t,R] = create2D2DOmniExperiment(pt_number,cam_number,noise,outlier_fraction);
[t_perturbed,R_perturbed] = perturb(t,R,0.01);
T_perturbed = [R_perturbed,t_perturbed];
T_init = [eye(3),zeros(3,1)];
T_gt = [R,t];
Out = opengv(algorithms{1},indices{1},v1,v2,T_perturbed);
rotation_error = evaluateRotationError( R, Out(1:3,1:3) );
rotation_errors(1,i) = rotation_error;
counter = counter + 1;
if counter == 100
counter = 0;
display(['Iteration ' num2str(i) ' of ' num2str(iterations) '(noise level ' num2str(noise) ')']);
end
end
%% Plot the results
hist(rad2deg(rotation_errors))
xlabel('rotation error [deg]')
ylabel('occurence')
grid on

51
thirdparty/opengv/matlab/plot_expected_iterations.m vendored Normal file
View File

@@ -0,0 +1,51 @@
%% Reset everything
clear all;
clc;
close all;
addpath('helpers');
%% Configure the benchmark
% The algorithms we want to test
algorithms = [ 6; 8; 17 ];
% The name of the algorithms in the final plots
names = { '6pt'; 'ge (8pt)'; '17pt'};
% The main experiment parameters
min_outlier_fraction = 0.05;%0.05;
max_outlier_fraction = 0.25;
outlier_fraction_step = 0.025;
p = 0.99;
%% Run the benchmark
%prepare the overall result arrays
number_outlier_fraction_levels = round((max_outlier_fraction - min_outlier_fraction) / outlier_fraction_step + 1);
num_algorithms = size(algorithms,1);
expected_number_iterations = zeros(num_algorithms,number_outlier_fraction_levels);
outlier_fraction_levels = zeros(1,number_outlier_fraction_levels);
%Run the experiment
for n=1:number_outlier_fraction_levels
outlier_fraction = (n - 1) * outlier_fraction_step + min_outlier_fraction;
outlier_fraction_levels(1,n) = outlier_fraction;
display(['Analyzing outlier fraction level: ' num2str(outlier_fraction)])
%Now compute the mean and median value of the error for each algorithm
for a=1:num_algorithms
expected_number_iterations(a,n) = log(1-p)/log(1-(1-outlier_fraction)^(algorithms(a,1)));
end
end
%% Plot the results
figure(1)
plot(outlier_fraction_levels,expected_number_iterations,'LineWidth',2)
legend(names,'Location','NorthWest')
xlabel('outlier fraction')
ylabel('expected number iterations')
grid on

210
thirdparty/opengv/matlab/plot_ge_costfunction.m vendored Normal file
View File

@@ -0,0 +1,210 @@
%% Reset everything
clear all;
clc;
close all;
addpath('helpers');
%% Configure the benchmark
% noncentral case
cam_number = 4;
% Getting 20 points
pt_number = 20;
% no outliers
outlier_fraction = 0.0;
% no noise
noise = 0.0;
[v1,v2,t,R] = create2D2DOmniExperiment(pt_number,cam_number,noise,outlier_fraction);
%% Plot the smallest Eigenvalue
evs = zeros(101,101);
for cay_z_index=1:5
cay_z = (cay_z_index - 3) * 0.2;
cay_z_index
for i=1:101
cay_x = 0.4*((i-51)/50);
i
for j=1:101
cay_y = 0.4*((j-51) / 50);
x = [cay_x;cay_y;cay_z];
if i == 51 && j == 51 && cay_z_index == 3
cay_y = 0.4*((52-51) / 50);
x = [cay_x;cay_y;cay_z];
end
Out = opengv('ge2',[1:1:size(v1,2)],v1,v2,[cayley2rot(x) zeros(3,1)]);
evs(i,j) = Out(4,5);
end
end
negativeIndices = evs<0;
evs(negativeIndices) = 0.0;
figure(1)
hold on
surf([-0.4 0.4],[-0.4 0.4],repmat(cay_z, [2 2]),log(evs),'facecolor','texture')
%colormap(gray);
view(45,30);
daspect([2.5 2.5 1]);
axis([-0.4 0.4 -0.4 0.4 -0.4 0.4])
grid on
xlabel('x');
ylabel('y');
zlabel('z');
colorbar
end
%% insert the zero layer
insertZeroLayer = 0;
if insertZeroLayer == 1
gt = rot2cayley(R);
cay_z = gt(3,1);
for i=1:101
cay_x = 0.4*((i-51)/50);
i
for j=1:101
cay_y = 0.4*((j-51) / 50);
x = [cay_x;cay_y;cay_z];
if i == 51 && j == 51 && cay_z_index == 3
cay_y = 0.4*((52-51) / 50);
x = [cay_x;cay_y;cay_z];
end
Out = opengv('ge2',[1:1:size(v1,2)],v1,v2,[cayley2rot(x) zeros(3,1)]);
evs(i,j) = Out(4,5);
end
end
negativeIndices = evs<0;
evs(negativeIndices) = 0.0;
figure(1)
hold on
surf([-0.4 0.4],[-0.4 0.4],repmat(cay_z, [2 2]),log(evs),'facecolor','texture')
%colormap(gray);
view(45,30);
daspect([2.5 2.5 1]);
axis([-0.4 0.4 -0.4 0.4 -0.4 0.4])
grid on
xlabel('x');
ylabel('y');
zlabel('z');
colorbar
end
%% Plot the second smallest Eigenvalue
evs = zeros(101,101);
for cay_z_index=1:5
cay_z = (cay_z_index - 3) * 0.2;
cay_z_index
for i=1:101
cay_x = 0.4*((i-51)/50);
i
for j=1:101
cay_y = 0.4*((j-51) / 50);
x = [cay_x;cay_y;cay_z];
if i == 51 && j == 51 && cay_z_index == 3
cay_y = 0.4*((52-51) / 50);
x = [cay_x;cay_y;cay_z];
end
Out = opengv('ge2',[1:1:size(v1,2)],v1,v2,[cayley2rot(x) zeros(3,1)]);
evs(i,j) = Out(3,5);
end
end
negativeIndices = evs<0;
evs(negativeIndices) = 0.0;
figure(2)
hold on
surf([-0.4 0.4],[-0.4 0.4],repmat(cay_z, [2 2]),log(evs),'facecolor','texture')
%colormap(gray);
view(45,30);
daspect([2.5 2.5 1]);
axis([-0.4 0.4 -0.4 0.4 -0.4 0.4])
grid on
xlabel('x');
ylabel('y');
zlabel('z');
colorbar
end
%% insert the zero layer
insertZeroLayer = 0;
if insertZeroLayer == 1
gt = rot2cayley(R);
cay_z = gt(3,1);
for i=1:101
cay_x = 0.4*((i-51)/50);
i
for j=1:101
cay_y = 0.4*((j-51) / 50);
x = [cay_x;cay_y;cay_z];
if i == 51 && j == 51 && cay_z_index == 3
cay_y = 0.4*((52-51) / 50);
x = [cay_x;cay_y;cay_z];
end
Out = opengv('ge2',[1:1:size(v1,2)],v1,v2,[cayley2rot(x) zeros(3,1)]);
evs(i,j) = Out(3,5);
end
end
negativeIndices = evs<0;
evs(negativeIndices) = 0.0;
figure(2)
hold on
surf([-0.4 0.4],[-0.4 0.4],repmat(cay_z, [2 2]),log(evs),'facecolor','texture')
%colormap(gray);
view(45,30);
daspect([2.5 2.5 1]);
axis([-0.4 0.4 -0.4 0.4 -0.4 0.4])
grid on
xlabel('x');
ylabel('y');
zlabel('z');
colorbar
end
%% Print the ground truth value on the console
minimum = rot2cayley(R)

78
thirdparty/opengv/matlab/ransac_experiment.m vendored Normal file
View File

@@ -0,0 +1,78 @@
%% Reset everything
clear all;
clc;
close all;
addpath('helpers');
%% Configure the benchmark
% noncentral case
cam_number = 4;
% set maximum and minimum number of points per cam
pt_number_per_cam = 50;
% set maximum and minimum number of outliers
min_outlier_fraction = 0.1;
max_outlier_fraction = 0.2;
% repeat 10000 iterations
iterations = 1000;
% The name of the algorithms in the final plots
names = { 'Homogeneous'; 'Vanilla' };
% The noise in this experiment
noise = 0.5;
%% Run the benchmark
%prepare the overall result arrays
ransac_iterations = zeros(2,iterations);
%Run the RANSAC with homogeneous sampling
counter = 0;
for i=1:iterations
%generate random outlier fraction
outlier_fraction = rand() * (max_outlier_fraction - min_outlier_fraction) + min_outlier_fraction;
% generate experiment
[v1,v2,cam_offsets,t,R] = createMulti2D2DExperiment(pt_number_per_cam,cam_number,noise,outlier_fraction);
Out = opengv_experimental1( v1{1,1}, v1{2,1}, v1{3,1}, v1{4,1}, v2{1,1}, v2{2,1}, v2{3,1}, v2{4,1}, cam_offsets, 2 );
ransac_iterations(1,i) = Out(1,5);
counter = counter + 1;
if counter == 100
counter = 0;
display(['Homogeneous sampling: Iteration ' num2str(i) ' of ' num2str(iterations)]);
end
end
%Run the RANSAC with vanilla sampling
counter = 0;
for i=1:iterations
%generate random outlier fraction
outlier_fraction = rand() * (max_outlier_fraction - min_outlier_fraction) + min_outlier_fraction;
% generate experiment
[v1,v2,t,R] = create2D2DExperiment(pt_number_per_cam*cam_number,cam_number,noise,outlier_fraction);
Out = opengv_experimental2( v1, v2, 2 );
ransac_iterations(2,i) = Out(1,5);
counter = counter + 1;
if counter == 100
counter = 0;
display(['Vanilla sampling: Iteration ' num2str(i) ' of ' num2str(iterations)]);
end
end
%% Plot the results
figure(1)
hist(ransac_iterations')
[Y,X] = hist(ransac_iterations')
legend(names,'Location','NorthEast')
xlabel('number of iterations')
grid on

92
thirdparty/opengv/matlab/ransac_experiment2.m vendored Normal file
View File

@@ -0,0 +1,92 @@
%% Reset everything
clear all;
clc;
close all;
addpath('helpers');
rng shuffle
%% Configure the benchmark
% noncentral case
cam_number = 4;
% Getting 10 points, and testing all algorithms with the respective number of points
pt_per_cam = 20;
% outlier test, so constant noise
noise = 0.5;
% repeat 100 tests per outlier-ratio
iterations = 50;
% The algorithms we want to test
algorithms = [ 0; 1; 2 ];
% The name of the algorithms in the final plots
names = { '6pt'; 'ge (8pt)'; '17pt'};
% The main experiment parameters
min_outlier_fraction = 0.05;
max_outlier_fraction = 0.45;
outlier_fraction_step = 0.05;
%% Run the benchmark
%prepare the overall result arrays
number_outlier_fraction_levels = round((max_outlier_fraction - min_outlier_fraction) / outlier_fraction_step + 1);
num_algorithms = size(algorithms,1);
%Run the experiment
for n=1:number_outlier_fraction_levels
outlier_fraction = (n - 1) * outlier_fraction_step + min_outlier_fraction;
display(['Analyzing outlier fraction level: ' num2str(outlier_fraction)])
clear number_iterations
clear execution_times
counter = 0;
temp_file_name1 = ['number_iterations_' num2str(outlier_fraction) '.mat'];
temp_file_name2 = ['execution_times_' num2str(outlier_fraction) '.mat'];
if exist(temp_file_name1,'file') > 0
display(['number_iterations_' num2str(outlier_fraction) '.mat exists already'])
load(temp_file_name1)
load(temp_file_name2)
startingIteration = size(number_iterations,2) + 1;
display(['starting at ' num2str(startingIteration)])
else
startingIteration = 1;
end
if startingIteration <= iterations
for i=startingIteration:iterations
% generate experiment
[v1,v2,cam_offsets,t,R] = createMulti2D2DOmniExperiment(pt_per_cam,cam_number,noise,outlier_fraction);
for a=1:num_algorithms
if strcmp(names{a,1},'6pt') && outlier_fraction > 0.25
Out = zeros(4,5);
time = 10000000.0;
else
tic
Out = opengv_experimental1( v1{1,1}, v1{2,1}, v1{3,1}, v1{4,1}, v2{1,1}, v2{2,1}, v2{3,1}, v2{4,1}, cam_offsets, algorithms(a,1) );
time = toc;
end
number_iterations(a,i) = Out(1,5);
execution_times(a,i) = time;
end
save(temp_file_name1,'number_iterations');
save(temp_file_name2,'execution_times');
counter = counter + 1;
if counter == 1
counter = 0;
display(['Iteration ' num2str(i) ' of ' num2str(iterations) '(outlier_fraction level ' num2str(outlier_fraction) ')']);
end
end
end
end

66
thirdparty/opengv/matlab/ransac_experiment3.m vendored Normal file
View File

@@ -0,0 +1,66 @@
%% Reset everything
clear all;
clc;
close all;
addpath('helpers');
%% Configure the benchmark
% The name of the algorithms in the final plots
names = { '6pt'; 'ge (8pt)'; '17pt'};
% The main experiment parameters
min_outlier_fraction = 0.05;
max_outlier_fraction = 0.45;
outlier_fraction_step = 0.05;
%% Run the benchmark
%prepare the overall result arrays
number_outlier_fraction_levels = round((max_outlier_fraction - min_outlier_fraction) / outlier_fraction_step + 1);
num_algorithms = size(names,1);
mean_number_iterations = zeros(num_algorithms,number_outlier_fraction_levels);
mean_execution_times = zeros(num_algorithms,number_outlier_fraction_levels);
outlier_fraction_levels = zeros(1,number_outlier_fraction_levels);
%Run the experiment
for n=1:number_outlier_fraction_levels
outlier_fraction = (n - 1) * outlier_fraction_step + min_outlier_fraction;
outlier_fraction_levels(1,n) = outlier_fraction;
display(['Analyzing outlier fraction level: ' num2str(outlier_fraction)])
clear number_iterations
clear execution_times
temp_file_name1 = ['number_iterations_' num2str(outlier_fraction) '.mat'];
temp_file_name2 = ['execution_times_' num2str(outlier_fraction) '.mat'];
load(temp_file_name1)
load(temp_file_name2)
%Now compute the mean and median value of the error for each algorithm
for a=1:num_algorithms
mean_number_iterations(a,n) = mean(number_iterations(a,:));
mean_execution_times(a,n) = mean(execution_times(a,:));
end
end
%% Plot the results
figure(1)
plot(outlier_fraction_levels,mean_number_iterations,'LineWidth',2)
legend(names,'Location','NorthWest')
xlabel('outlier fraction')
ylabel('mean number iterations')
axis([0.05 0.25 0 1500])
grid on
figure(2)
hold on
plot(outlier_fraction_levels(1,1:6),mean_execution_times(1,1:6),'LineWidth',2)
plot(outlier_fraction_levels,mean_execution_times(2:3,:),'LineWidth',2)
legend(names,'Location','NorthWest')
xlabel('outlier fraction')
ylabel('mean execution time [s]')
axis([0.05 0.45 0 40])
grid on

17
thirdparty/opengv/matlab/ransac_test.m vendored Normal file
View File

@@ -0,0 +1,17 @@
%% Reset everything
clear all;
clc;
close all;
addpath('helpers');
% central case -> only one camera
cam_number = 1;
% let's only get 6 points, and generate new ones in each iteration
pt_number = 100;
% noise test, so no outliers
outlier_fraction = 0.1;
noise = 0.0;
[points,v,t,R] = create2D3DExperiment(pt_number,cam_number,noise,outlier_fraction);
[X, inliers] = opengv('p3p_kneip_ransac',points,v);