-
Notifications
You must be signed in to change notification settings - Fork 1
/
plotCorrespondences.m
97 lines (90 loc) · 3.86 KB
/
plotCorrespondences.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
function plotCorrespondences(opts, axes_h, cur_axes, centroid, ...
correspondences, H, s, title_txt)
num_points = size(correspondences,2);
color_list = getColors(3);
if ~isfield(opts.plotting, 'plane_boundary')
opts.plotting.plane_boundary = 10;
end
if ~isempty(centroid)
scatter3(axes_h(cur_axes), ...
centroid.x(1), centroid.x(2), centroid.x(3), 'fill', 'rd')
end
marker_size = 10;
for i = 1:num_points
if isa(correspondences(i).model, 'Plane')
continue
end
% source measurements
if opts.plotting.source_points
plot3(axes_h(cur_axes), ...
correspondences(i).point.x(1), ...
correspondences(i).point.x(2), ...
correspondences(i).point.x(3), ...
'color', color_list{3}, ...
'MarkerFaceColor', color_list{3}, ...
'Marker', 'o', 'MarkerSize', marker_size);
end
% corrected measurements
transformed = H * correspondences(i).point;
if opts.plotting.corrected_source
plot3(axes_h(cur_axes), ...
transformed.x(1), ...
transformed.x(2), ...
transformed.x(3), ...
'color', color_list{3}, ...
'MarkerFaceColor', color_list{3},...
'Marker', 'o', 'MarkerSize', marker_size);
end
if opts.plotting.model
if isa(correspondences(i).model, 'Point')
plot3(axes_h(cur_axes), ...
correspondences(i).model.x(1), ...
correspondences(i).model.x(2), ...
correspondences(i).model.x(3), ...
'color', color_list{1}, ...
'MarkerFaceColor', color_list{1}, ...
'Marker', '^', 'MarkerSize', marker_size);
elseif isa(correspondences(i).model, 'Line')
correspondences(i).model.plot(axes_h(cur_axes));
% plotALine(axes_h(cur_axes), direction, point, start_, end_)
elseif isa(correspondences(i).model, 'Plane')
plotAPlaneWithNormalNPoint(axes_h(cur_axes), ...
correspondences(i).model.n, ...
correspondences(i).model.x, ...
-opts.plotting.plane_boundary, ...
opts.plotting.plane_boundary, 0.5)
else
assert("Not a type from Plane, Line, Point")
end
end
% distance from corrected source to target
if opts.plotting.corrected2target
if isa(correspondences(i).model, 'Point')
plotArrow(axes_h(cur_axes), ...
correspondences(i).model.x, transformed.x)
elseif isa(correspondences(i).model, 'Plane') || ...
isa(correspondences(i).model, 'Line')
plotArrow(axes_h(cur_axes), ...
correspondences(i).model.project(transformed).x, ...
transformed.x)
else
assert("Not a type from Plane, Line, Point")
end
end
% distance from source to target
if opts.plotting.source2target
if isa(correspondences(i).model, 'Point')
plotArrow(axes_h(cur_axes), ...
correspondences(i).model.x, correspondences(i).point.x)
elseif isa(correspondences(i).model, 'Plane') || ...
isa(correspondences(i).model, 'Line')
plotArrow(axes_h(cur_axes), ...
correspondences(i).model.project(transformed).x, ...
correspondences(i).point.x)
else
assert("Not a type from Plane, Line, Point")
end
end
end
viewCurrentPlot(axes_h(cur_axes), title_txt, [145, 5], 0);
end