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 |
---|