[37] | 1 | %VGG_SOLVELIN_BLKSYM Solves M*x==y where M is (typically huge sparse) symmetric 4-block matrix.
|
---|
| 2 | % It solves the system much more efficiently than a general (sparse) linear system solver.
|
---|
| 3 | % Typical usage to solve normal equations in Levenberg-Marquardt in bundle adjustment.
|
---|
| 4 | %
|
---|
| 5 | % X = VGG_SOLVELIN_BLKSYM(A,B,C,p,q [,'nocheck']) solves M*x==Y where
|
---|
| 6 | % M=[A B; B' C] and y=[p;q].
|
---|
| 7 | %
|
---|
| 8 | % X = VGG_SOLVELIN_BLKSYM(M,y,sideA [,'nocheck']) does the same but takes M and splits it,
|
---|
| 9 | % M=[A B; B' C] and size(A)==[sideA sideA].
|
---|
| 10 | %
|
---|
| 11 | % The function checks whether C is really near to diagonal; if not, a warning is printed.
|
---|
| 12 | % Parmeter 'nocheck' switches off this test.
|
---|
| 13 |
|
---|
| 14 | % (c) {awf,werner}@robots.ox.ac.uk, March 2002
|
---|
| 15 |
|
---|
| 16 |
|
---|
| 17 | function x = vgg_solvelin_blksym(varargin)
|
---|
| 18 |
|
---|
| 19 | switch nargin
|
---|
| 20 | case 6
|
---|
| 21 | [A,B,C,p,q,check] = deal(varargin{:});
|
---|
| 22 | case 5
|
---|
| 23 | [A,B,C,p,q] = deal(varargin{:});
|
---|
| 24 | check = '';
|
---|
| 25 | case 4
|
---|
| 26 | [M,y,sideA,check] = deal(varargin{:});
|
---|
| 27 | case 3
|
---|
| 28 | [M,y,sideA] = deal(varargin{:});
|
---|
| 29 | check = '';
|
---|
| 30 | otherwise
|
---|
| 31 | error('Bad number of parameters');
|
---|
| 32 | end
|
---|
| 33 |
|
---|
| 34 | if nargin<5
|
---|
| 35 | Rtop = 1:sideA;
|
---|
| 36 | Rbot = sideA+1:size(M,1);
|
---|
| 37 | C = M(Rbot, Rbot);
|
---|
| 38 | B = M(Rtop, Rbot);
|
---|
| 39 | A = M(Rtop, Rtop);
|
---|
| 40 | p = y(Rtop);
|
---|
| 41 | q = y(Rbot);
|
---|
| 42 | end
|
---|
| 43 |
|
---|
| 44 | if ~strcmp(check,'nocheck') & nnz(C(1,:))/size(C,1) > .1
|
---|
| 45 | warning('It is likely that M was splitted incorrectly. Check diagonality of C and/or value of sideA.');
|
---|
| 46 | end
|
---|
| 47 |
|
---|
| 48 | %tic
|
---|
| 49 |
|
---|
| 50 | % Surprisingly, branch 1 is 2x slower than branch 2. We don't know why.
|
---|
| 51 | switch 2
|
---|
| 52 | case 1
|
---|
| 53 | invC_Btq = C \ [B' q];
|
---|
| 54 | case 2
|
---|
| 55 | invC_Btq = inv(C) * [B' q];
|
---|
| 56 | end
|
---|
| 57 |
|
---|
| 58 | invC_Bt = invC_Btq(:,1:end-1);
|
---|
| 59 | invC_q = invC_Btq(:,end);
|
---|
| 60 |
|
---|
| 61 | u = (A - B * invC_Bt) \ (p - B * invC_q);
|
---|
| 62 | v = invC_q - invC_Bt*u;
|
---|
| 63 |
|
---|
| 64 | x = [u;v];
|
---|
| 65 |
|
---|
| 66 | %toc
|
---|
| 67 |
|
---|
| 68 | return |
---|