[37] | 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 |
---|