improve m-estimator visualization code
Varun Agrawal
parent
65b309e5cd
commit
f58d421b71
|
|
@ -1,172 +1,102 @@
|
|||
import gtsam.*
|
||||
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
|
||||
% GTSAM Copyright 2010, Georgia Tech Research Corporation,
|
||||
% Atlanta, Georgia 30332-0415
|
||||
% All Rights Reserved
|
||||
% Authors: Frank Dellaert, et al. (see THANKS for the full author list)
|
||||
%
|
||||
% See LICENSE for the license information
|
||||
%
|
||||
% @brief Plot visualizations of residuals, residual derivatives, and weights for the various mEstimators.
|
||||
% @author Varun Agrawal
|
||||
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
|
||||
|
||||
% import gtsam.*
|
||||
|
||||
clear all;
|
||||
close all;
|
||||
|
||||
x = linspace(-10, 10, 1000);
|
||||
% x = linspace(-10, 10, 21)
|
||||
% x = linspace(-5, 5, 101);
|
||||
|
||||
[rho, psi, w] = fair(x);
|
||||
figure(1);
|
||||
plot_rho(x, rho, 1, -1, 30)
|
||||
title('Fair');
|
||||
plot_psi(x, psi, 2, -3, 3);
|
||||
plot_w(x, w, 3, 3);
|
||||
saveas(figure(1), 'fair.png');
|
||||
c = 1.3998;
|
||||
rho = fair(x, c);
|
||||
fairNoiseModel = gtsam.noiseModel.mEstimator.Fair(c);
|
||||
plot_m_estimator(x, fairNoiseModel, rho, 'Fair', 1, 'fair.png')
|
||||
|
||||
[rho, psi, w] = huber(x);
|
||||
figure(2);
|
||||
plot_rho(x, rho, 1, -1, 30);
|
||||
title('Huber');
|
||||
plot_psi(x, psi, 2, -15, 15);
|
||||
plot_w(x, w, 3, 5);
|
||||
saveas(figure(2), 'huber.png');
|
||||
c = 1.345;
|
||||
rho = huber(x, c);
|
||||
huberNoiseModel = gtsam.noiseModel.mEstimator.Huber(c);
|
||||
plot_m_estimator(x, huberNoiseModel, rho, 'Huber', 2, 'huber.png')
|
||||
|
||||
[rho, psi, w] = cauchy(x);
|
||||
figure(3);
|
||||
plot_rho(x, rho, 1, -0.1, 0.1);
|
||||
title('Cauchy');
|
||||
plot_psi(x, psi, 2, -0.2, 0.2);
|
||||
plot_w(x, w, 3, 1.5);
|
||||
saveas(figure(3), 'cauchy.png');
|
||||
c = 0.1;
|
||||
rho = cauchy(x, c);
|
||||
cauchyNoiseModel = gtsam.noiseModel.mEstimator.Cauchy(c);
|
||||
plot_m_estimator(x, cauchyNoiseModel, rho, 'Cauchy', 3, 'cauchy.png')
|
||||
|
||||
[rho, psi, w] = gemancclure(x);
|
||||
figure(4);
|
||||
plot_rho(x, rho, 1, -1, 1);
|
||||
title('Geman-McClure');
|
||||
plot_psi(x, psi, 2, -1, 1);
|
||||
plot_w(x, w, 3, 1.5);
|
||||
saveas(figure(4), 'gemanmcclure.png');
|
||||
c = 1.0;
|
||||
rho = gemanmcclure(x, c);
|
||||
gemanmcclureNoiseModel = gtsam.noiseModel.mEstimator.GemanMcClure(c);
|
||||
plot_m_estimator(x, gemanmcclureNoiseModel, rho, 'GemanMcClure', 4, 'gemanmcclure.png')
|
||||
|
||||
[rho, psi, w] = welsch(x);
|
||||
figure(5);
|
||||
plot_rho(x, rho, 1, -5, 10);
|
||||
title('Welsch');
|
||||
plot_psi(x, psi, 2, -2, 2);
|
||||
plot_w(x, w, 3, 2);
|
||||
saveas(figure(5), 'welsch.png');
|
||||
c = 2.9846;
|
||||
rho = welsch(x, c);
|
||||
welschNoiseModel = gtsam.noiseModel.mEstimator.Welsch(c);
|
||||
plot_m_estimator(x, welschNoiseModel, rho, 'Welsch', 5, 'welsch.png')
|
||||
|
||||
[rho, psi, w] = tukey(x);
|
||||
figure(6);
|
||||
plot_rho(x, rho, 1, -5, 10);
|
||||
title('Tukey');
|
||||
plot_psi(x, psi, 2, -2, 2);
|
||||
plot_w(x, w, 3, 2);
|
||||
saveas(figure(6), 'tukey.png');
|
||||
c = 4.6851;
|
||||
rho = tukey(x, c);
|
||||
tukeyNoiseModel = gtsam.noiseModel.mEstimator.Tukey(c);
|
||||
plot_m_estimator(x, tukeyNoiseModel, rho, 'Tukey', 6, 'tukey.png')
|
||||
|
||||
function plot_rho(x, y, idx, yll, ylu)
|
||||
subplot(3, 1, idx);
|
||||
plot(x, y);
|
||||
xlim([-15, 15]);
|
||||
ylim([yll, ylu]);
|
||||
end
|
||||
|
||||
function plot_psi(x, y, idx, yll, ylu)
|
||||
subplot(3, 1, idx);
|
||||
plot(x, y);
|
||||
xlim([-15, 15]);
|
||||
ylim([yll, ylu]);
|
||||
end
|
||||
|
||||
function plot_w(x, y, idx, yl)
|
||||
subplot(3, 1, idx);
|
||||
plot(x, y);
|
||||
xlim([-15, 15]);
|
||||
ylim([-yl, yl]);
|
||||
end
|
||||
|
||||
function [rho, psi, w] = fair(x)
|
||||
import gtsam.*
|
||||
c = 1.3998;
|
||||
|
||||
rho = c^2 * ( (abs(x) ./ c) - log(1 + (abs(x)./c)) );
|
||||
est = noiseModel.mEstimator.Fair(c);
|
||||
|
||||
%% Plot rho, psi and weights of the mEstimator.
|
||||
function plot_m_estimator(x, model, rho, plot_title, fig_id, filename)
|
||||
w = zeros(size(x));
|
||||
for i = 1:size(x, 2)
|
||||
w(i) = est.weight(x(i));
|
||||
w(i) = model.weight(x(i));
|
||||
end
|
||||
|
||||
psi = w .* x;
|
||||
|
||||
figure(fig_id);
|
||||
subplot(3, 1, 1);
|
||||
plot(x, rho);
|
||||
xlim([-5, 5]);
|
||||
title(plot_title);
|
||||
subplot(3, 1, 2);
|
||||
plot(x, psi);
|
||||
xlim([-5, 5]);
|
||||
subplot(3, 1, 3);
|
||||
plot(x, w);
|
||||
xlim([-5, 5]);
|
||||
saveas(figure(fig_id), filename);
|
||||
end
|
||||
|
||||
function [rho, psi, w] = huber(x)
|
||||
import gtsam.*
|
||||
k = 5;
|
||||
function rho = fair(x, c)
|
||||
rho = c^2 * ( (abs(x) ./ c) - log(1 + (abs(x)./c)) );
|
||||
end
|
||||
|
||||
function rho = huber(x, k)
|
||||
t = (abs(x) > k);
|
||||
|
||||
rho = (x .^ 2) ./ 2;
|
||||
rho(t) = k * (abs(x(t)) - (k/2));
|
||||
est = noiseModel.mEstimator.Huber(k);
|
||||
|
||||
w = zeros(size(x));
|
||||
for i = 1:size(x, 2)
|
||||
w(i) = est.weight(x(i));
|
||||
end
|
||||
|
||||
psi = w .* x;
|
||||
end
|
||||
|
||||
function [rho, psi, w] = cauchy(x)
|
||||
import gtsam.*
|
||||
c = 0.1;
|
||||
|
||||
function rho = cauchy(x, c)
|
||||
rho = (c^2 / 2) .* log(1 + ((x./c) .^ 2));
|
||||
|
||||
est = noiseModel.mEstimator.Cauchy(c);
|
||||
|
||||
w = zeros(size(x));
|
||||
for i = 1:size(x, 2)
|
||||
w(i) = est.weight(x(i));
|
||||
end
|
||||
|
||||
psi = w .* x;
|
||||
end
|
||||
|
||||
function [rho, psi, w] = gemancclure(x)
|
||||
import gtsam.*
|
||||
c = 1.0;
|
||||
function rho = gemanmcclure(x, c)
|
||||
rho = ((x .^ 2) ./ 2) ./ (1 + x .^ 2);
|
||||
|
||||
est = noiseModel.mEstimator.GemanMcClure(c);
|
||||
|
||||
w = zeros(size(x));
|
||||
for i = 1:size(x, 2)
|
||||
w(i) = est.weight(x(i));
|
||||
end
|
||||
|
||||
psi = w .* x;
|
||||
end
|
||||
|
||||
function [rho, psi, w] = welsch(x)
|
||||
import gtsam.*
|
||||
c = 2.9846;
|
||||
function rho = welsch(x, c)
|
||||
rho = (c^2 / 2) * ( 1 - exp(-(x ./ c) .^2 ));
|
||||
|
||||
est = noiseModel.mEstimator.Welsch(c);
|
||||
|
||||
w = zeros(size(x));
|
||||
for i = 1:size(x, 2)
|
||||
w(i) = est.weight(x(i));
|
||||
end
|
||||
|
||||
psi = w .* x;
|
||||
end
|
||||
|
||||
function [rho, psi, w] = tukey(x)
|
||||
import gtsam.*
|
||||
c = 4.6851;
|
||||
function rho = tukey(x, c)
|
||||
t = (abs(x) > c);
|
||||
|
||||
rho = (c^2 / 6) * (1 - (1 - (x ./ c) .^ 2 ) .^ 3 );
|
||||
rho(t) = c .^ 2 / 6;
|
||||
|
||||
est = noiseModel.mEstimator.Tukey(c);
|
||||
|
||||
w = zeros(size(x));
|
||||
for i = 1:size(x, 2)
|
||||
w(i) = est.weight(x(i));
|
||||
end
|
||||
|
||||
psi = w .* x;
|
||||
end
|
||||
|
|
|
|||
Loading…
Reference in New Issue