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