Chain test on Cholesky

gtsam_unstable/testing_tools/base/cholChainTest.m

@@ -0,0 +1,25 @@
+scale = 1e-10;
+length = 10000;
+
+A = spalloc(length, length, 2*length);
+b = zeros(length, 1);
+
+A(1,1) = 1;
+
+for i = 2:length
+    A(i, i-1) = 1e2*scale;
+    A(i, i) = -scale;
+end
+
+L = A'*A;
+eta = A'*b;
+
+R = choleskyNaive(L);
+clear opts
+opts.LT = true;
+d = R' \ eta; %linsolve(R', eta, opts);
+
+clear opts
+opts.UT = true;
+x = R \ d; %linsolve(R, d, opts);
+

gtsam_unstable/testing_tools/base/choleskyNaive.m

@@ -4,6 +4,7 @@ function A = choleskyNaive(A)
 
 % Loop over rows of A
 q = min(size(A,2), size(A,1));
+lastA = 0;
 for i = 1:q
     % Check for valid pivot
     if A(i,i) <= 0.0
@@ -11,6 +12,11 @@ for i = 1:q
         return;
     end
     
+    if lastA == A(i,i)
+        1;
+    end
+    lastA = A(i,i);
+    
     % 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);