From f58d421b71836dbb850913fcf55356282595d18a Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Tue, 24 Sep 2019 12:48:38 -0400 Subject: [PATCH] improve m-estimator visualization code --- matlab/gtsam_examples/VisualizeMEstimators.m | 196 ++++++------------- 1 file changed, 63 insertions(+), 133 deletions(-) diff --git a/matlab/gtsam_examples/VisualizeMEstimators.m b/matlab/gtsam_examples/VisualizeMEstimators.m index 8fce828aa..5120934fe 100644 --- a/matlab/gtsam_examples/VisualizeMEstimators.m +++ b/matlab/gtsam_examples/VisualizeMEstimators.m @@ -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