Added simple matrix-math eliminate and shortcut functions, and a simple matrix-math test of the feasibility of correcting root shortcut joint marginals.

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
+

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], [])

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
+