766_mosaic/RANSAC.m at master · erickulcyk/766_mosaic · GitHub

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
function [bestTransform] = RANSAC( feature1, feature2, epsilon, iter, onlyTranslation)
f1count = size(feature1, 2);
f2count = size(feature2, 2);
if f1count<4 || f2count < 4
    disp('Not enough features to do ransac');
end

i = 1;
bestInliers = 0;
A = zeros(8,8);
b = zeros(8);
bestTransform = 0;
while i<iter
    i= i+1;
    if mod(i,10)==0
        disp(i)
        disp(['Best: ',num2str(bestInliers)])
    end

    %Compute Pairs
    [ pts1 ] = FourRandomNumbers(f1count);
    if onlyTranslation==0
        % Compute Homography
        for j =1:4
            A(j,1) = int32(feature1(1,pts1(j)));
            A(j,2) = int32(feature1(2,pts1(j)));
            A(j,3) = 1;

            A(j,4) = 0;
            A(j,5) = 0;
            A(j,6) = 0;

            A(j,7) = -int32(feature2(1,pts1(j))) * int32(feature1(1,pts1(j)));
            A(j,8) = -int32(feature2(1,pts1(j))) * int32(feature1(2,pts1(j)));

            b(j) = int32(feature2(1,pts1(j)));
        end

        for j =1:4
            A(j+4,1) = 0;
            A(j+4,2) = 0;
            A(j+4,3) = 0;

            A(j+4,4) = int32(feature1(1,pts1(j)));
            A(j+4,5) = int32(feature1(2,pts1(j)));
            A(j+4,6) = 1;

            A(j+4,7) = -int32(feature2(2,pts1(j))) * int32(feature1(1,pts1(j)));
            A(j+4,8) = -int32(feature2(2,pts1(j))) * int32(feature1(2,pts1(j)));

            b(j+4) = int32(feature2(2,pts1(j)));
        end

        h = A\b;
        %Check Points
        H = zeros(3,3);
        H(1,1) = h(1);
        H(1,2) = h(2);
        H(1,3) = h(3);
        H(2,1) = h(4);
        H(2,2) = h(5);
        H(2,3) = h(6);
        H(3,1) = h(7);
        H(3,2) = h(8);
        H(3,3) = 1;

    else

        H = zeros(3,3);
        H(1,1) = 1;
        H(1,2) = 0;
        H(1,3) = feature2(1,pts1(1))-feature1(1,pts1(1));
        H(2,1) = 0;
        H(2,2) = 1;
        H(2,3) = feature2(2,pts1(1))-feature1(2,pts1(1));
        H(3,1) = 0;
        H(3,2) = 0;
        H(3,3) = 1;

    end

    inliers = 0;
    for j=1:size(feature1,2)
        pt = [feature1(1:2,j);1];
        opt = H*pt;
        img2Pt = int32(round(opt/opt(3)));
        dist = (img2Pt(1)-feature2(1,j))^2 + (img2Pt(2)-feature2(2,j))^2;
        if dist<epsilon
            inliers = inliers + 1;
        end
    end

    if inliers>bestInliers
        bestInliers = inliers;
        bestTransform = H;
        disp('bestTransform: ');
        disp(bestTransform);
    end
end
tmp = bestTransform(1,3);
bestTransform(1,3) = bestTransform(2,3);
bestTransform(2,3) = tmp;

tmp = bestTransform(1,2);
bestTransform(1,2) = bestTransform(2,1);
bestTransform(2,1) = tmp;

tmp = bestTransform(1,1);
bestTransform(1,1) = bestTransform(2,2);
bestTransform(2,2) = tmp;

disp('bestTransform: ');
disp(bestTransform);
end