-
Notifications
You must be signed in to change notification settings - Fork 5
/
TrifocalEstimate.m
122 lines (105 loc) · 3.32 KB
/
TrifocalEstimate.m
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
115
116
117
118
119
120
121
122
close all;
clear all;
clc;
img1 = imread('1.JPG');
img1_ = img1;
img1 = rgb2gray(img1);
img2 = imread('2.JPG');
img2_ = img2;
img2 = rgb2gray(img2);
img3 = imread('3.JPG');
img3_ = img3;
img3 = rgb2gray(img3);
points1 = detectHarrisFeatures(img1);
points2 = detectHarrisFeatures(img2);
points3 = detectHarrisFeatures(img3);
[f1, vpts1] = extractFeatures(img1, points1);
[f2, vpts2] = extractFeatures(img2, points2);
[f3, vpts3] = extractFeatures(img3, points3);
pairs12 = matchFeatures(f1, f2);
pairs23 = matchFeatures(f2, f3);
matchedPoints12_1 = vpts1(pairs12(:,1));
matchedPoints12_2 = vpts2(pairs12(:,2));
matchedPoints23_2 = vpts2(pairs23(:,1));
matchedPoints23_3 = vpts3(pairs23(:,2));
matchedTriplets = [];
%count = 0;
for i=1:size(pairs12,1),
match = find(pairs23(:,1) == pairs12(i,2));
if(size(match,1)~= 0)
matchedTriplets = [matchedTriplets; pairs12(i,1), pairs12(i,2), pairs23(match, 2)];
end;
end;
[Tri, matchper, m, m1] = RANSACTrifocal(vpts1, vpts2, vpts3, matchedTriplets);
for i=1:6,
m(i) = MLEupdate(Tri, m(i));
end;
shapeInserter = vision.ShapeInserter('Shape', 'Rectangles', 'BorderColor', 'Custom', 'CustomBorderColor', uint8([255 120 0]), 'Fill', true, 'FillColor', 'Custom', 'CustomFillColor', uint8([255, 0, 0]));
figure(1)
RGB1 = img1_;
hold on;
for i=1:6,
circle1 = int16([m(i).a1(1) m(i).a1(2) 20 20]);
RGB1 = step(shapeInserter, RGB1, circle1);
%RGB1 = insertMarker(RGB1,int16(m(i).a1(1:2)'),'x', 'color', 'white', 'size', 10);
end;
imshow(RGB1);
figure(2)
hold on;
RGB2 = img2_;
for i=1:6,
circle2 = int16([m(i).a2(1) m(i).a2(2) 20 20]);
RGB2 = step(shapeInserter, RGB2, circle2);
%RGB2 = insertMarker(RGB2,int16(m(i).a2(1:2)'),'x', 'color', 'white', 'size', 10);
end;
imshow(RGB2);
figure(3)
hold on;
RGB3 = img3_;
for i=1:6,
circle3 = int16([m(i).a3(1) m(i).a3(2) 20 20]);
RGB3 = step(shapeInserter, RGB3, circle3);
%RGB3 = insertMarker(img3_,int16(m(i).a3(1:2)'),'x', 'color', 'white', 'size', 10);
end;
imshow(RGB3);
for i=1:6,
p3 = pointTransfer(Tri, m1(i).a1, m1(i).a2);
p3 = p3/p3(3);
m1(i).a3 = p3;
end;
figure(1);
hold on;
shapeInserter = vision.ShapeInserter('Shape', 'Rectangles', 'BorderColor', 'Custom', 'CustomBorderColor', uint8([0 120 255]), 'Fill', true, 'FillColor', 'Custom', 'CustomFillColor', uint8([0, 0, 255]));
for i=1:6,
circle1 = int16([m1(i).a1(1) m1(i).a1(2) 20 20]);
RGB1 = step(shapeInserter, RGB1, circle1);
%RGB1 = insertMarker(RGB1,int16(m(i).a1(1:2)'),'x', 'color', 'white', 'size', 10);
end;
imshow(RGB1);
figure(2)
hold on;
for i=1:6,
circle2 = int16([m1(i).a2(1) m1(i).a2(2) 20 20]);
RGB2 = step(shapeInserter, RGB2, circle2);
%RGB2 = insertMarker(RGB2,int16(m(i).a2(1:2)'),'x', 'color', 'white', 'size', 10);
end;
imshow(RGB2);
figure(3)
hold on;
for i=1:6,
circle3 = int16([m1(i).a3(1) m1(i).a3(2) 20 20]);
RGB3 = step(shapeInserter, RGB3, circle3);
%RGB3 = insertMarker(img3_,int16(m(i).a3(1:2)'),'x', 'color', 'white', 'size', 10);
end;
imshow(RGB3);
%{
figure(3)
hold on;
imshow(step(markerInserter,img1,[int16(m(1).a3(1)), int16(m(1).a3(2))]) );
%plot(m(1).a1(3), m(1).a2(3), 'Marker','p','Color',[.88 .48 0],'MarkerSize',20);
%}
%%
%figure(1);
%showMatchedFeatures(img1, img2, matchedPoints12_1, matchedPoints12_2);
%figure(2);
%showMatchedFeatures(img2, img3, matchedPoints23_2, matchedPoints23_3);