From 00b12c7dc1238406d1b7a76055076c3ccca0541f Mon Sep 17 00:00:00 2001 From: Richard Roberts Date: Mon, 29 Oct 2012 15:52:02 +0000 Subject: [PATCH] Added simple matrix-math eliminate and shortcut functions, and a simple matrix-math test of the feasibility of correcting root shortcut joint marginals. --- .../testing_tools/inference/eliminate.m | 45 +++++++++++ .../inference/jointMarginalsTestProblems.m | 76 +++++++++++++++++++ .../testing_tools/inference/shortcut.m | 20 +++++ 3 files changed, 141 insertions(+) create mode 100644 gtsam_unstable/testing_tools/inference/eliminate.m create mode 100644 gtsam_unstable/testing_tools/inference/jointMarginalsTestProblems.m create mode 100644 gtsam_unstable/testing_tools/inference/shortcut.m diff --git a/gtsam_unstable/testing_tools/inference/eliminate.m b/gtsam_unstable/testing_tools/inference/eliminate.m new file mode 100644 index 000000000..05d87b6cf --- /dev/null +++ b/gtsam_unstable/testing_tools/inference/eliminate.m @@ -0,0 +1,45 @@ +function [ RS, J ] = eliminate(Ab, js) +%ELIMINATE This is a simple scalar-level function for eliminating the +%matrix Ab into a conditional part RS and a marginal part J. The scalars +%in js are the ones eliminated (corresponding to columns of Ab). Note that +%the returned matrices RS and J will both be the same width as Ab and will +%contain columns of all zeros - that is, the column indices will stay the +%same in all matrices, unlike in GTSAM where matrix data is only stored for +%involved variables. + +if size(Ab,1) < size(Ab,2)-1 + Ab = [ Ab; zeros(size(Ab,2)-1-size(Ab,1), size(Ab,2)) ]; +end + +% Permute columns +origCols = 1:size(Ab,2); +jsFront = js; +jsBack = setdiff(origCols, js); +jsPermuted = [ jsFront jsBack ]; +Abpermuted = Ab(:,jsPermuted); + +% Eliminate (this sparse stuff prevents qr from introducing a permutation +R = full(qr(sparse(Abpermuted))); + +% Find row split +firstRowOfMarginal = numel(jsFront) + 1; +for i = size(R,1) : -1 : 1 + firstNnz = find(R(i,:), 1, 'first'); + if ~isempty(firstNnz) + if firstNnz > numel(jsFront) + firstRowOfMarginal = i; + else + break; + end + end +end + +% Undo permutation +Runpermuted(:,jsPermuted) = R; + +% Split up +RS = Runpermuted(1:firstRowOfMarginal-1, :); +J = Runpermuted(firstRowOfMarginal:size(Ab,2)-1, :); + +end + diff --git a/gtsam_unstable/testing_tools/inference/jointMarginalsTestProblems.m b/gtsam_unstable/testing_tools/inference/jointMarginalsTestProblems.m new file mode 100644 index 000000000..9a8dd38f4 --- /dev/null +++ b/gtsam_unstable/testing_tools/inference/jointMarginalsTestProblems.m @@ -0,0 +1,76 @@ +% This script tests the math of correcting joint marginals with root +% shortcuts, using only simple matrix math and avoiding any GTSAM calls. +% It turns out that it is not possible to correct root shortcuts for +% joinoint marginals, thus actual_ij_1_2__ will not equal +% expected_ij_1_2__. + +%% Check that we get the BayesTree structure we are expecting +fg = gtsam.SymbolicFactorGraph; +fg.push_factor(1,3,7,10); +fg.push_factor(2,5,7,10); +fg.push_factor(3,4,7,8); +fg.push_factor(5,6,7,8); +fg.push_factor(7,8,9,10); +fg.push_factor(10,11); +fg.push_factor(11); +fg.push_factor(7,8,9); +fg.push_factor(7,8); +fg.push_factor(5,6); +fg.push_factor(3,4); + +bt = gtsam.SymbolicMultifrontalSolver(fg).eliminate; +bt + +%% +% P( 10 11) +% P( 7 8 9 | 10) +% P( 5 6 | 7 8 10) +% P( 2 | 5 7 10) +% P( 3 4 | 7 8 10) +% P( 1 | 3 7 10) +A = [ ... + % 1 2 3 4 5 6 7 8 9 10 11 b + 1 0 2 0 0 0 3 0 0 4 0 5 + 0 6 0 0 7 0 8 0 0 9 0 10 + 0 0 11 12 0 0 13 14 0 15 0 16 + 0 0 0 0 17 18 19 20 0 21 0 22 + 0 0 0 0 0 0 23 24 25 26 0 27 + 0 0 0 0 0 0 0 0 0 28 29 30 + 0 0 0 0 0 0 0 0 0 0 31 32 + 0 0 0 0 0 0 33 34 35 0 0 36 + 0 0 0 0 0 0 37 38 0 0 0 39 + 0 0 0 0 40 41 0 0 0 0 0 42 + 0 0 43 44 0 0 0 0 0 0 0 45 + %0 0 0 0 46 0 0 0 0 0 0 47 + %0 0 48 0 0 0 0 0 0 0 0 49 + ]; + +% Compute shortcuts +shortcut3_7__10_11 = shortcut(A, [3,7], [10,11]); +shortcut5_7__10_11 = shortcut(A, [5,7], [10,11]); + +% Factor into pieces +% Factor p(5,7|10,11) into p(5|7,10,11) p(7|10,11) +[ si_si__sij_R, si_sij__R ] = eliminate(shortcut3_7__10_11, [3]); +[ sj_sj__sij_R, sj_sij__R ] = eliminate(shortcut5_7__10_11, [5]); + +% Get root marginal +R__ = A([6,7], :); + +% Get clique conditionals +i_1__3_7_10 = A(1, :); +j_2__5_7_10 = A(2, :); + +% Check +expected_ij_1_2__ = shortcut(A, [1,2], []) + +% Compute joint +ij_1_2_3_5_7_10_11 = [ + i_1__3_7_10 + j_2__5_7_10 + si_si__sij_R + sj_sj__sij_R + si_sij__R + R__ + ]; +actual_ij_1_2__ = shortcut(ij_1_2_3_5_7_10_11, [1,2], []) diff --git a/gtsam_unstable/testing_tools/inference/shortcut.m b/gtsam_unstable/testing_tools/inference/shortcut.m new file mode 100644 index 000000000..9e1940afd --- /dev/null +++ b/gtsam_unstable/testing_tools/inference/shortcut.m @@ -0,0 +1,20 @@ +function R = shortcut(Ab, frontals, separators) +%SHORTCUT This is a simple scalar-level function for computing a "shortcut" +%of frontals conditioned on separators. Note that the input matrix Ab is +%assumed to have a RHS vector in its right-most column, thus this rightmost +%column index should not be listed in either frontals or separators. This +%function computes p(frontals | separators), marginalizing out all other +%variables (cooresponding to columns) in Ab. + +% First marginalize out all others +cols = 1:size(Ab,2)-1; +toMarginalizeOut = setdiff(cols, [ frontals separators ]); +[ cond marg ] = eliminate(Ab, toMarginalizeOut); + +% Now eliminate the frontals to get the conditional +[ cond marg ] = eliminate(marg, frontals); + +R = cond; + +end +