-
Notifications
You must be signed in to change notification settings - Fork 0
/
Copy pathTrimPointCloud.m
173 lines (142 loc) · 6.39 KB
/
TrimPointCloud.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
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
149
150
151
152
153
154
155
156
157
158
159
160
161
162
163
164
165
166
167
168
169
170
171
172
173
function [TrimmedCloud] = TrimPointCloud (Cloud, Shape, Center, Size, Plot)
% TRIMPOINTCLOUD trims the point cloud given as input by a sphere, a
% cube, a prism or an ellipsoid. The trimming operation is based on the
% distance to the specified center position
%% Inputs
% Cloud Input point cloud
% Shape Can be an integer or a string specifying the shape that
% will be trimmed: 1 - 'Cube', 2 - 'Sphere', 3 - 'Prism', 4 -
% 'Ellipsoid'
% Center 1-by-3 or 3-by-1 vector with XYZ coordinates of center
% Size Size of the shape that will be trimmed. If you selected the
% Cube or the Sphere, you only have to give one dimension,
% which is the side length or the radius, respectively. If
% you chose the Prism or the Ellipsoid, you should specify
% three dimensions, being them the side lengths or the
% radiuses, respectively, along the three cartesian axes
% Plot Use 'Yes' to plot the main figures and 'No' to ignore them
%% Output
% TrimmedCloud Trimmed point cloud
%% Input Parsing
% Errors
if ((isnumeric (Shape) && Shape == 3) || strcmpi (Shape, 'Prism')) ...
&& numel (Size) ~= 3
error (strcat ('Prism requires three dimensions, which are the', ...
' side lengths.'));
end
if ((isnumeric (Shape) && Shape == 4) || strcmpi (Shape, 'Ellipsoid')) ...
&& numel (Size) ~= 3
error (strcat ('Ellipsoid requires three dimensions, which are', ...
' the radiuses along the XYZ axes.'));
end
% Warnings
if ((isnumeric (Shape) && Shape == 1) || strcmpi (Shape, 'Cube')) ...
&& numel (Size) > 1
warning (strcat ('Cube only needs one dimensions, which is the', ...
' side length. This function will, therefore, use only the', ...
' first specified dimension.'));
end
if ((isnumeric (Shape) && Shape == 2) || strcmpi (Shape, 'Sphere')) ...
&& numel (Size) > 1
warning (strcat ('Sphere only needs one dimensions, which is the', ...
' radius. This function will, therefore, use only the first', ...
' specified dimension.'));
end
%% Preallocation of Variables
Coordinates = double (Cloud.Location);
Dimensionality = numel (size (Coordinates));
if Dimensionality == 3
Norms = zeros (size (Coordinates, 1), size (Coordinates, 2));
UsefullPoints = zeros (size (Coordinates, 1), size (Coordinates, 2));
elseif Dimensionality == 2
Norms = zeros (1, size (Coordinates, 1));
UsefullPoints = zeros (1, size (Coordinates, 1));
end
%% Trimming Operation
% Cube
if ((isnumeric (Shape) && Shape == 1) || strcmpi (Shape, 'Cube'))
Size = Size (1) / 2;
Limits = zeros (2, 3);
Limits (1, 1 : 3) = Center (1 : 3) - Size;
Limits (2, 1 : 3) = Center (1 : 3) + Size;
UsefulIndices = findPointsInROI (Cloud, Limits');
% Sphere
elseif ((isnumeric (Shape) && Shape == 2) || strcmpi (Shape, 'Sphere'))
if Dimensionality == 3
for i = 1 : size (Coordinates, 1)
for j = 2 : size (Coordinates, 2)
Point = Coordinates (i, j, :);
Norms (i, j) = pdist ([(Point (:))'; ...
Center(1) Center(2) Center(3)]);
end
end
elseif Dimensionality == 2
for i = 1 : size (Coordinates, 1)
Point = Coordinates (i, :);
Norms (i) = pdist ([Point; Center(1) Center(2) Center(3)]);
end
end
UsefulIndices = find (Norms (:) <= Size (1));
% Prism
elseif ((isnumeric (Shape) && Shape == 3) || strcmpi (Shape, 'Prism'))
Size = Size ./ 2;
Limits = zeros (2, 3);
Limits (1, 1 : 3) = Center (1 : 3) - Size (1 : 3);
Limits (2, 1 : 3) = Center (1 : 3) + Size (1 : 3);
UsefulIndices = findPointsInROI (Cloud, Limits');
% Ellipsoid
elseif ((isnumeric (Shape) && Shape == 4) || strcmpi (Shape, 'Ellipsoid'))
% The implict equation of the ellipsoid has the following standard
% form: (x^2)/(a^2) + (y^2)/(b^2) + (z^2)/(c^2) = 1, being a, b and c
% the radiuses along the XYZ axes. Since the points of interest are in
% the ellipsoid and inside it, the equal to signal is replaced by the
% smaller than or equal to signal
if Dimensionality == 3
for i = 1 : size (Coordinates, 1)
for j = 2 : size (Coordinates, 2)
Point = Coordinates (i, j, :);
UsefullPoints (i, j) = ( ...
((Center (1) - Point (1)) ^ 2) / (Size (1) ^ 2) + ...
((Center (2) - Point (2)) ^ 2) / (Size (2) ^ 2) + ...
((Center (3) - Point (3)) ^ 2) / (Size (3) ^ 2)) <= 1;
end
end
elseif Dimensionality == 2
for i = 1 : size (Coordinates, 1)
Point = Coordinates (i, :);
UsefullPoints (i) = ( ...
((Center (1) - Point (1)) ^ 2) / (Size (1) ^ 2) + ...
((Center (2) - Point (2)) ^ 2) / (Size (2) ^ 2) + ...
((Center (3) - Point (3)) ^ 2) / (Size (3) ^ 2)) <= 1;
end
end
UsefulIndices = find (UsefullPoints (:) == 1);
end
TrimmedCloud = select (Cloud, UsefulIndices);
%% Plotting
if strcmpi (Plot, 'Yes')
figure ('Name', 'BACKGROUND REMOVAL', 'NumberTitle', 'off');
MyAxes = pcshow (TrimmedCloud);
MyAxes.XTick = [-0.3 -0.2 -0.1 0 0.1 0.2 0.3];
MyAxes.XTickLabelMode = 'manual';
MyAxes.XTickLabel = {'', '', '', '', '', '', ''};
MyAxes.YTick = [-0.3 -0.2 -0.1 0 0.1 0.2 0.3];
MyAxes.YTickLabelMode = 'manual';
MyAxes.YTickLabel = {'', '', '', '', '', '', ''};
MyAxes.ZTick = [0.5 0.6 0.7 0.8 0.9 1 1.1 1.2 1.3 1.4 1.5];
MyAxes.ZTickLabelMode = 'manual';
MyAxes.ZTickLabel = {'', '', '', '', '', '', '', '', '', '', ''};
MyAxes.CameraPositionMode = 'manual';
MyAxes.CameraPosition = [-10, -25, -35];
MyAxes.CameraUpVector = [0 -1 0];
% print (gcf, '2_BackgroundRemoval.emf', ...
% '-dmeta', '-r300', '-painters');
% xlabel ('X [m]'); ylabel ('Y [m]'); zlabel ('Z [m]');
elseif strcmpi (Plot, 'Test')
figure ('Name', 'BACKGROUND REMOVAL', 'NumberTitle', 'off');
MyAxes = pcshow (TrimmedCloud);
MyAxes.CameraPositionMode = 'manual';
MyAxes.CameraPosition = [-10, -25, -35];
MyAxes.CameraUpVector = [0 -1 0];
end
end