From 60f29fde159fec00708b2967eab221f241b5639d Mon Sep 17 00:00:00 2001
From: Richard Roberts <richard.jw.roberts@gmail.com>
Date: Fri, 23 Nov 2012 23:24:58 +0000
Subject: [PATCH] MATLAB tests for Cholesky scaling and preconditioning

---
 .../testing_tools/base/cholScalingTest.m      | 100 ++++++++++++++++++
 .../testing_tools/base/choleskyNaive.m        |  27 +++++
 2 files changed, 127 insertions(+)
 create mode 100644 gtsam_unstable/testing_tools/base/cholScalingTest.m
 create mode 100644 gtsam_unstable/testing_tools/base/choleskyNaive.m

diff --git a/gtsam_unstable/testing_tools/base/cholScalingTest.m b/gtsam_unstable/testing_tools/base/cholScalingTest.m
new file mode 100644
index 000000000..16aed1684
--- /dev/null
+++ b/gtsam_unstable/testing_tools/base/cholScalingTest.m
@@ -0,0 +1,100 @@
+
+badscale = 1e-14;
+
+% Create random Jacobian
+A = [
+    badscale.*(rand(3)-.5) zeros(3)   zeros(3,6)
+    zeros(3) (rand(3)-.5)             zeros(3,6)
+    badscale.*(rand(2,6)-.5)          zeros(2,6)
+    zeros(2,6)                        badscale.*(rand(2,6)-.5)
+    eye(6)                            -eye(6)
+    ];
+b = [
+    repmat(badscale, 3,1)
+    repmat(1, 3,1)
+    repmat(badscale, 2,1)
+    repmat(badscale, 2,1)
+    repmat(1, 6,1)
+    ];
+
+% Solve with MATLAB
+x_true = A \ b;
+
+% Form Hessian
+L = A'*A;
+l = A'*b;
+
+% Eliminate badly-conditioned system.  Use this custom choleskyNaive function
+% that does no internal pivoting or scaling to simulate the case where the
+% bad conditioning occurs between fronts.
+R = choleskyNaive(L);
+if isempty(R)
+    fprintf('Cholesky failed on badly-scaled system with a negative pivot\n');
+else
+    clear opts
+    opts.LT = true;
+    d = linsolve(R', l, opts);
+
+    % Solve badly-conditioned system.
+    clear opts
+    opts.UT = true;
+    x = linsolve(R, d, opts);
+    
+    % Compute error
+    fprintf('Solution error with Cholesky on badly-scaled system: %g\n', ...
+        max(abs(x - x_true)));
+end
+
+% Form scaled system from Jacobian
+%Si = full(qr(sparse(A)));
+%Si = diag(sqrt(diag(L)));
+S = [
+   eye(6) zeros(6)
+   eye(6) eye(6) ];
+Ap = A * S;
+Lp = Ap'*Ap;
+lp = Ap'*b;
+
+% Eliminate scaled system
+Rp = choleskyNaive(Lp);
+if isempty(Rp)
+    fprintf('Cholesky failed on scaled system with a negative pivot\n');
+else
+    clear opts
+    opts.LT = true;
+    dp = linsolve(Rp', lp, opts);
+
+    % Solve rescaled system.
+    clear opts
+    opts.UT = true;
+    xp = S * linsolve(Rp, dp, opts);
+    
+    % Compute error
+    fprintf('Solution error with Cholesky from rescaled Jacobian: %g\n', ...
+        max(abs(xp - x_true)));
+end
+
+
+% Form scaled system from Hessian
+Lph = S' * (L * S);
+lph = S' * l;
+
+% Eliminate scaled system
+Rph = choleskyNaive(Lph);
+if isempty(Rph)
+    fprintf('Cholesky failed on scaled system with a negative pivot\n');
+else
+    clear opts
+    opts.LT = true;
+    dph = linsolve(Rph', lph, opts);
+
+    % Solve rescaled system.
+    clear opts
+    opts.UT = true;
+    xph = S * linsolve(Rph, dph, opts);
+    
+    % Compute error
+    fprintf('Solution error with Cholesky from rescaled Hessian: %g\n', ...
+        max(abs(xph - x_true)));
+end
+
diff --git a/gtsam_unstable/testing_tools/base/choleskyNaive.m b/gtsam_unstable/testing_tools/base/choleskyNaive.m
new file mode 100644
index 000000000..6591ac529
--- /dev/null
+++ b/gtsam_unstable/testing_tools/base/choleskyNaive.m
@@ -0,0 +1,27 @@
+function A = choleskyNaive(A)
+%ELIMINATEPP Gaussian elimination with partial pivoting.  Eliminates a
+%matrix A (sparse or full) into an upper-triangular factor R.
+
+% Loop over rows of A
+q = min(size(A,2), size(A,1));
+for i = 1:q
+    % Check for valid pivot
+    if A(i,i) <= 0.0
+        A = [];
+        return;
+    end
+    
+    % Scale row to be the square-root factor
+    A(i,i) = sqrt(A(i,i));
+    A(i,i+1:end) = A(i,i+1:end) ./ A(i,i);
+    
+    % Apply low-rank update to remanining lower-right submatrix
+    A(i+1:end, i+1:end) = A(i+1:end, i+1:end) - ...
+       A(i, i+1:end)' * A(i, i+1:end);
+    
+    % Zero-out the column below the current row
+    A(i+1:end, i) = 0;
+end
+
+end
+