1 | % By Philip Torr 2002
|
---|
2 | % copyright Microsoft Corp.
|
---|
3 | function f1 = torr_display_structure(X, P1, P2,display_numbers,f1)
|
---|
4 |
|
---|
5 |
|
---|
6 | if nargin < 4
|
---|
7 | display_numbers = 0;
|
---|
8 | end
|
---|
9 |
|
---|
10 | if nargin < 5
|
---|
11 | f1 = figure
|
---|
12 | else
|
---|
13 | figure(f1)
|
---|
14 | end
|
---|
15 |
|
---|
16 |
|
---|
17 | [M,N] = size(X);
|
---|
18 |
|
---|
19 | if M==4 %then normalize if homogeneous...
|
---|
20 | X(1,:) = X(1,:) ./ X(4,:);
|
---|
21 | X(2,:) = X(2,:) ./ X(4,:);
|
---|
22 | X(3,:) = X(3,:) ./ X(4,:);
|
---|
23 | X(4,:) = X(4,:) ./ X(4,:);
|
---|
24 | end
|
---|
25 |
|
---|
26 | %next we want (for display) to remove points at infinity as they will be difficult to display
|
---|
27 | %this is done by calculating the centroid of the points and removing ones that are too far away
|
---|
28 | %relative to the camera baseline
|
---|
29 | baseline = norm([P2(1,4) P2(2,4) P2(3,4)]);
|
---|
30 | nearby_index = find(X(3,:) < 40 * baseline);
|
---|
31 |
|
---|
32 | plot3(X(1,nearby_index),X(2,nearby_index),X(3,nearby_index),'rs');
|
---|
33 |
|
---|
34 | % if display_numbers
|
---|
35 | % mat_index1 = 1:N;
|
---|
36 | % mat_index1 = mat_index1';
|
---|
37 | % mat_index = num2str(mat_index1);
|
---|
38 | % text(X(1,:),X(2,:),X(3,:),mat_index)
|
---|
39 | % end
|
---|
40 | %
|
---|
41 | if display_numbers
|
---|
42 | mat_index1 = nearby_index;
|
---|
43 | mat_index1 = mat_index1';
|
---|
44 | mat_index = num2str(mat_index1);
|
---|
45 | text(X(1,nearby_index),X(2,nearby_index),X(3,nearby_index),mat_index)
|
---|
46 | end
|
---|
47 |
|
---|
48 |
|
---|
49 | hold on
|
---|
50 | plot3(0,0,0,'gs');
|
---|
51 | plot3(P2(1,4),P2(2,4),P2(3,4),'gs');
|
---|
52 | hold off
|
---|
53 |
|
---|