diff --git a/matlab/gtsam_examples/VisualizeMEstimators.m b/matlab/gtsam_examples/VisualizeMEstimators.m new file mode 100644 index 000000000..b8ffc03c4 --- /dev/null +++ b/matlab/gtsam_examples/VisualizeMEstimators.m @@ -0,0 +1,160 @@ +import gtsam.* + +clear all; +close all; + +x = linspace(-10, 10, 1000); +% x = linspace(-10, 10, 21) + +[rho, psi, w] = fair(x); +plot_rho(x, rho, 1, -1, 30) +title('Fair'); +plot_psi(x, psi, 7, -3, 3); +plot_w(x, w, 13, 3); + +[rho, psi, w] = huber(x); +plot_rho(x, rho, 2, -1, 30); +title('Huber'); +plot_psi(x, psi, 8, -15, 15); +plot_w(x, w, 14, 5); + +[rho, psi, w] = cauchy(x); +plot_rho(x, rho, 3, -0.1, 0.1); +title('Cauchy'); +plot_psi(x, psi, 9, -0.2, 0.2); +plot_w(x, w, 15, 1.5); + +[rho, psi, w] = gemancclure(x); +plot_rho(x, rho, 4, -1, 1); +title('Geman-McClure'); +plot_psi(x, psi, 10, -1, 1); +plot_w(x, w, 16, 1.5); + +[rho, psi, w] = welsch(x); +plot_rho(x, rho, 5, -5, 10); +title('Welsch'); +plot_psi(x, psi, 11, -2, 2); +plot_w(x, w, 17, 2); + +[rho, psi, w] = tukey(x); +plot_rho(x, rho, 6, -5, 10); +title('Tukey'); +plot_psi(x, psi, 12, -2, 2); +plot_w(x, w, 18, 2); + +function plot_rho(x, y, idx, yll, ylu) + subplot(3, 6, idx); + plot(x, y); + xlim([-15, 15]); + ylim([yll, ylu]); +end + +function plot_psi(x, y, idx, yll, ylu) + subplot(3, 6, idx); + plot(x, y); + xlim([-15, 15]); + ylim([yll, ylu]); +end + +function plot_w(x, y, idx, yl) + subplot(3, 6, 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); + + 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] = huber(x) + import gtsam.* + k = 5; + 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; + + 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; + 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; + rho = (c^2 / 2) * ( 1 - exp(-(x ./ c) .^2 )); + + est = noiseModel.mEstimator.Welsh(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; + 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