-
Notifications
You must be signed in to change notification settings - Fork 2
/
findLineAsCorrespondence.m
101 lines (98 loc) · 4.08 KB
/
findLineAsCorrespondence.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
function correspondences = findLineAsCorrespondence(opts, ...
original_pc, template_lines, axes_h)
if ~exist('axes_h', 'var') || isempty(axes_h)
opts.debug == 0;
end
if opts.debug == 2
opts.simulate_lidar = 0;
opts.arrow_length = 0.5;
plotObjectsList(axes_h, opts, original_pc.LiDAR_opts, ideal_object_list);
colors = getColors(4);
for i = 1:4
scatter3(ideal_object_list.object_vertices.x(i), ...
ideal_object_list.object_vertices.y(i), ...
ideal_object_list.object_vertices.z(i), ...
'MarkerFaceColor', colors{i}, ...
'MarkerEdgeColor', colors{i})
end
end
num_points = size(original_pc.object_list.points_mat, 2);
correspondences = repmat(Point2Line(), 1, ...
opts.ncorrespondences.num_per_point*num_points);
num_lines = size(template_lines.line, 2);
for i = 1:num_points
if opts.ncorrespondences.num_per_point == 4
indx = opts.ncorrespondences.num_per_point * (i-1) + 1;
point = Point(original_pc.object_list.centered_points(1:3, i));
for j = 1:num_lines
correspondences(indx + j - 1) = Point2Line(...
point, template_lines.line(j));
end
elseif opts.ncorrespondences.num_per_point == 2
indx = opts.ncorrespondences.num_per_point * (i-1) + 1;
point = Point(original_pc.object_list.centered_points(1:3, i));
farthest_dis = -1;
farthest_indx = 0;
closest_dis = 1e5;
closest_indx = 0;
for j = 1:num_lines
dis = template_lines.line(j).distSq(point);
if (dis < closest_dis)
closest_dis = dis;
closest_indx = j;
end
if (dis > farthest_dis)
farthest_dis = dis;
farthest_indx = j;
end
end
correspondences(indx) = Point2Line(...
point, template_lines.line(closest_indx));
correspondences(indx+1) = Point2Line(...
point, template_lines.line(farthest_indx));
if opts.debug == 2
h1 = scatter3(axes_h, point.x(1), point.x(2), point.x(3), 'm*');
h2 = correspondences(indx).model.plot(axes_h, rand(1,3));
h3 = correspondences(indx+1).model.plot(axes_h, rand(1,3));
viewCurrentPlot(axes_h, "process", [-90, 0], 1)
delete(h1);
delete(h2);
delete(h3);
end
elseif opts.ncorrespondences.num_per_point == 1
point = Point(original_pc.object_list.centered_points(1:3, i));
if opts.ncorrespondences.mode == 1
closest_dis = 1e5;
closest_indx = 0;
for j = 1:num_lines
dis = template_lines.line(j).distSq(point);
if (dis < closest_dis)
closest_dis = dis;
closest_indx = j;
end
end
correspondences(i) = Point2Line(...
point, template_lines.line(closest_indx));
elseif opts.ncorrespondences.mode == 2
farthest_dis = -1;
farthest_indx = 0;
for j = 1:num_lines
dis = template_lines.line(j).distSq(point);
if (dis > farthest_dis)
farthest_dis = dis;
farthest_indx = j;
end
end
correspondences(i) = Point2Line(...
point, template_lines.line(farthest_indx));
end
if opts.debug == 2
h1 = scatter3(axes_h, point.x(1), point.x(2), point.x(3), 'm*');
h2 = correspondences(i).model.plot(axes_h, rand(1,3));
viewCurrentPlot(axes_h, "process", [-90, 0], 1)
delete(h1);
delete(h2);
end
end
end
end