[37] | 1 | % By Philip Torr 2002
|
---|
| 2 | % copyright Microsoft Corp.
|
---|
| 3 | %generates a set of point matches + their F matrix!@
|
---|
| 4 |
|
---|
| 5 | %we need to make sure that the object isnt too affine
|
---|
| 6 |
|
---|
| 7 | %default values
|
---|
| 8 |
|
---|
| 9 | foc = 256.0;
|
---|
| 10 | no_matches = 100;
|
---|
| 11 | noise_multiplier = 1;
|
---|
| 12 | translation_mult = foc * 2;
|
---|
| 13 | translation_adder = 0;
|
---|
| 14 |
|
---|
| 15 | %max number of degrees to rotate
|
---|
| 16 | rotation_multplier = 2;
|
---|
| 17 | min_Z = 1;
|
---|
| 18 | Z_RAN = 10;
|
---|
| 19 |
|
---|
| 20 |
|
---|
| 21 | m3 = 256.0;
|
---|
| 22 | C = eye(3);
|
---|
| 23 | C(1,3) = 0;
|
---|
| 24 | C(2,3) = 0;
|
---|
| 25 | C(3,3) = 1/foc;
|
---|
| 26 |
|
---|
| 27 | R = eye(3);
|
---|
| 28 | t = rand(3,1);
|
---|
| 29 | t = t * (translation_mult *norm(t)) +translation_adder;
|
---|
| 30 | T = [0 -t(3) t(2); t(3) 0 -t(1); -t(2) t(1) 0];
|
---|
| 31 | T = T/norm(T);
|
---|
| 32 |
|
---|
| 33 | theta = 1/360 * 2 * pi * rand * rotation_multplier;
|
---|
| 34 | n = 1/360 * 2 * pi * rand * rotation_multplier;
|
---|
| 35 | p = 1/360 * 2 * pi * rand * rotation_multplier;
|
---|
| 36 |
|
---|
| 37 | R(1,1) = (1 - cos(p)) * cos(n)* ( cos(n) * cos(theta) + sin(theta) * sin(n) ) + cos(p)* cos(theta);
|
---|
| 38 | R(1,2) = (1 - cos(p))* cos(n) * ( sin(n) *cos(theta) - sin(theta) *cos(n) ) - cos(p) *sin(theta);
|
---|
| 39 | R(1,3) = sin(n) *sin(p);
|
---|
| 40 | R(2,1) = (1 - cos(p)) *sin(n) *( cos(n) *cos(theta) + sin(theta)* sin(n) ) + cos(p)* sin(theta);
|
---|
| 41 | R(2,2) = (1 - cos(p)) *sin(n) * ( sin(n) *cos(theta) - sin(theta) *cos(n) ) + cos(p)* cos(theta);
|
---|
| 42 | R(2,3) = -cos(n) * sin(p);
|
---|
| 43 | R(3,1) = -sin(p) * ( sin(n) * cos(theta) - sin(theta) * cos(n));
|
---|
| 44 | R(3,2) = sin(p) * ( cos(n) * cos(theta) + sin(theta) * sin(n));
|
---|
| 45 | R(3,3) = cos(p);
|
---|
| 46 |
|
---|
| 47 | %R * R'
|
---|
| 48 | %R' * R
|
---|
| 49 |
|
---|
| 50 | perfect_matches = rand(no_matches,4);
|
---|
| 51 | noisy_matches = rand(no_matches,4);
|
---|
| 52 | m1 = rand(3,no_matches);
|
---|
| 53 | m2 = rand(3,no_matches);
|
---|
| 54 |
|
---|
| 55 | x1 = rand(no_matches,1);
|
---|
| 56 | x2 = rand(no_matches,1);
|
---|
| 57 | y1 = rand(no_matches,1);
|
---|
| 58 | y2 = rand(no_matches,1);
|
---|
| 59 | u1 = rand(no_matches,1);
|
---|
| 60 | v1 = rand(no_matches,1);
|
---|
| 61 | X = rand(3,no_matches);
|
---|
| 62 |
|
---|
| 63 | %noisy data:----
|
---|
| 64 | nx1 = rand(no_matches,1);
|
---|
| 65 | nx2 = rand(no_matches,1);
|
---|
| 66 | ny1 = rand(no_matches,1);
|
---|
| 67 | ny2 = rand(no_matches,1);
|
---|
| 68 | nu1 = rand(no_matches,1);
|
---|
| 69 | nv1 = rand(no_matches,1);
|
---|
| 70 |
|
---|
| 71 | for(i = 1:no_matches)
|
---|
| 72 | X(3,i) = min_Z * foc + Z_RAN * foc * X(3,i);
|
---|
| 73 | X(1,i) = (X(1,i) * 512 -256 ) *X(3,i)/foc;
|
---|
| 74 | X(2,i) = (X(2,i) * 512 -256 ) * X(3,i)/foc ;
|
---|
| 75 | end
|
---|
| 76 |
|
---|
| 77 |
|
---|
| 78 | m1 = C *X;
|
---|
| 79 | m2 = R *X;
|
---|
| 80 | m2 = m2 + t * ones(1,no_matches);
|
---|
| 81 | m2 = C * m2;
|
---|
| 82 | % m2 = C * ( R *X + t);
|
---|
| 83 |
|
---|
| 84 | %for(i = 1:no_matches)
|
---|
| 85 | x1 = (m1(1,:)./m1(3,:))';
|
---|
| 86 | y1 = (m1(2,:)./m1(3,:))';
|
---|
| 87 | x2 = (m2(1,:)./m2(3,:))';
|
---|
| 88 | y2 = (m2(2,:)./m2(3,:))';
|
---|
| 89 |
|
---|
| 90 |
|
---|
| 91 | perfect_matches(:,1) = x1(:);
|
---|
| 92 | perfect_matches(:,2) = y1(:);
|
---|
| 93 | perfect_matches(:,3) = x2(:);
|
---|
| 94 | perfect_matches(:,4) = y2(:);
|
---|
| 95 |
|
---|
| 96 | u1(:) = x2(:) - x1(:);
|
---|
| 97 | v1(:) = y2(:) - y1(:);
|
---|
| 98 |
|
---|
| 99 |
|
---|
| 100 |
|
---|
| 101 | noisy_matches(:,1) = x1(:) + randn(no_matches,1) *noise_multiplier;
|
---|
| 102 | noisy_matches(:,2) = y1(:) + randn(no_matches,1) *noise_multiplier;
|
---|
| 103 | noisy_matches(:,3) = x2(:) + randn(no_matches,1) *noise_multiplier;
|
---|
| 104 | noisy_matches(:,4) = y2(:) + randn(no_matches,1) *noise_multiplier;
|
---|
| 105 |
|
---|
| 106 | nx1 = noisy_matches(:,1);
|
---|
| 107 | ny1 = noisy_matches(:,2);
|
---|
| 108 | nx2 = noisy_matches(:,3);
|
---|
| 109 | ny2 = noisy_matches(:,4);
|
---|
| 110 | nu1 = nx2(:) - nx1(:);
|
---|
| 111 | nv1 = ny2(:) - ny1(:);
|
---|
| 112 |
|
---|
| 113 |
|
---|
| 114 | %m2(i)' * F * m1(i)
|
---|
| 115 |
|
---|
| 116 | F = inv(C') * T * R * inv(C);
|
---|
| 117 |
|
---|
| 118 | %this is the wrong answer for the algebraic residual!! m1(i)' * F * m2(i)
|
---|
| 119 |
|
---|
| 120 | %note here m3 = 1, so need to adjust this to check the results...
|
---|
| 121 | MM = [1 0 0; 0 1 0; 0 0 1/m3];
|
---|
| 122 | MMC = MM * inv(C);
|
---|
| 123 |
|
---|
| 124 | %F2 = MM * F * MM;
|
---|
| 125 | F2 = MMC * T * R * MMC;
|
---|
| 126 | true_F = F2 / norm(F2);
|
---|
| 127 | %
|
---|
| 128 | % %i think this is the right one
|
---|
| 129 | % f_true = [F2(1) F2(2,:) F2(3,:)];
|
---|
| 130 | % f_true = f_true/norm(f_true);
|
---|
| 131 | %
|
---|
| 132 | %
|
---|
| 133 | % %wrong one
|
---|
| 134 | % fff = [F(:,1)' F(:,2)' F(:,3)']
|
---|
| 135 | % fff = fff/norm(fff);
|
---|
| 136 |
|
---|
| 137 | %e = errf2(fff,x1,y1,x2,y2, no_matches, m3);
|
---|
| 138 |
|
---|
| 139 | tf = true_F;
|
---|
| 140 | %tf = F;
|
---|
| 141 | 0.5 * (trace(tf * tf'))^2 - trace((tf * tf')^2)
|
---|
| 142 |
|
---|
| 143 | tf * (tf' * tf) - 0.5 .* (trace(tf * tf') * tf)
|
---|
| 144 |
|
---|
| 145 | svd(true_F)
|
---|
| 146 | eig(true_F) |
---|