1 | function [error, Mapprox, stable] = rankrsfm(M,INC) |
---|
2 | % rankr is a generic routine for fitting linear surfaces. This routine is |
---|
3 | % designed the case of structure from motion, with translation. This differs |
---|
4 | % from the generic case in two ways. First, translation implies that M has |
---|
5 | % rank r+1 (typically 4, unless motion is somehow limited; in this routine |
---|
6 | % we will assume r=3, since this influences column selection strategies), and that |
---|
7 | % one of the basis vectors for this space is a row vector of (1 1 1 ... 1). |
---|
8 | % Second, in structure from motion there must be a specific pattern of |
---|
9 | % missing data, since the x and y coordinates of a point are either both present, |
---|
10 | % or both missing. |
---|
11 | |
---|
12 | % For the second reason, we will consider every pair of frames, rather than |
---|
13 | % looking for larger combinations of points. |
---|
14 | |
---|
15 | % We then have two possible strategies, depending on whether we work with the |
---|
16 | % matrix or its transpose. |
---|
17 | |
---|
18 | MAXNULLSIZE = 10; |
---|
19 | [numrows,numcols] = size(M); |
---|
20 | NULLSPACE = []; |
---|
21 | frame_pairs = random_frame_pairs(numrows/2); |
---|
22 | % gives a 2 x (numframes choose 2) matrix with all pairs of frames, in random order. |
---|
23 | |
---|
24 | for pair = frame_pairs |
---|
25 | if size(NULLSPACE,2) >= numrows*MAXNULLSIZE, break, end |
---|
26 | |
---|
27 | [submatrix,subcols] = fp_submatrix(pair,M,INC); |
---|
28 | subrows = [2*pair(1,1)-1, 2*pair(1,1), 2*pair(2,1)-1, 2*pair(2,1)]; |
---|
29 | % Right now, I'm only doing nullspace of matrix, not it's transpose. |
---|
30 | num_subpoints = size(submatrix,2); |
---|
31 | if num_subpoints > 3 |
---|
32 | submatrix = [ones(1,num_subpoints); submatrix]; |
---|
33 | subnull = nulleps(submatrix,.001,4); |
---|
34 | if size(subnull,2) == 1 |
---|
35 | % Since submatrix has 5 rows and 4+ columns, if it has rank 4, subnull is 1 col. |
---|
36 | % If subnull is bigger, submatrix is rank deficient, and results unstable. |
---|
37 | nullTEMP = zeros(numrows+1, 1); |
---|
38 | nullTEMP([1,1+subrows],:) = subnull; |
---|
39 | NULLSPACE = [NULLSPACE, nullTEMP]; |
---|
40 | end |
---|
41 | end |
---|
42 | end |
---|
43 | |
---|
44 | %NULLSPACE |
---|
45 | %error = NULLSPACE; |
---|
46 | %return; |
---|
47 | |
---|
48 | % size(NULLSPACE) |
---|
49 | % rank(NULLSPACE) |
---|
50 | % global NULLSPACEexternal |
---|
51 | % NULLSPACEexternal = NULLSPACE; |
---|
52 | |
---|
53 | if size(NULLSPACE) == [0 0] |
---|
54 | error = -2; |
---|
55 | Mapprox = []; |
---|
56 | else |
---|
57 | [error, Mapprox, stable1, stable2] ... |
---|
58 | = approximate([ones(1,numcols);M],[ones(1,numcols);INC],4,NULLSPACE); |
---|
59 | % This works, but it's not clear that the best use of knowledge that there's |
---|
60 | % translation is to just add a row of 1's. One possibility is to make |
---|
61 | % this row have large magnitude, so that it will be very well approximated |
---|
62 | % (since it has no noise). Another is to figure out a correct method. |
---|
63 | if error ~= -2 |
---|
64 | Mapprox = Mapprox(2:numrows+1,:); |
---|
65 | error = sum(sum((M.*INC - Mapprox.*INC).^2)); |
---|
66 | end |
---|
67 | % In determining error, error in this row shouldn't be counted. |
---|
68 | stable = stable1 & stable2 |
---|
69 | end |
---|
70 | |
---|
71 | if error == -2 |
---|
72 | stable = 0; |
---|
73 | end |
---|