added script to visualize values of different mEstimators

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