1 // ----------------------------------------------------------------------------
2 // - Open3D: www.open3d.org -
3 // ----------------------------------------------------------------------------
4 // The MIT License (MIT)
5 //
6 // Copyright (c) 2018 www.open3d.org
7 //
8 // Permission is hereby granted, free of charge, to any person obtaining a copy
9 // of this software and associated documentation files (the "Software"), to deal
10 // in the Software without restriction, including without limitation the rights
11 // to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
12 // copies of the Software, and to permit persons to whom the Software is
13 // furnished to do so, subject to the following conditions:
14 //
15 // The above copyright notice and this permission notice shall be included in
16 // all copies or substantial portions of the Software.
17 //
18 // THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
19 // IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
20 // FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
21 // AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
22 // LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING
23 // FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS
24 // IN THE SOFTWARE.
25 // ----------------------------------------------------------------------------
26
27 #include "PointCloud.h"
28 #include "TriangleMesh.h"
29
30 #include <unordered_map>
31
32 #include <Core/Utility/Helper.h>
33 #include <Core/Utility/Console.h>
34
35 namespace three{
36
37 namespace {
38
39 class AccumulatedPoint
40 {
41 public:
AccumulatedPoint()42 AccumulatedPoint() :
43 num_of_points_(0),
44 point_(0.0, 0.0, 0.0),
45 normal_(0.0, 0.0, 0.0),
46 color_(0.0, 0.0, 0.0)
47 {
48 }
49
50 public:
AddPoint(const PointCloud & cloud,int index)51 void AddPoint(const PointCloud &cloud, int index)
52 {
53 point_ += cloud.points_[index];
54 if (cloud.HasNormals()) {
55 if (!std::isnan(cloud.normals_[index](0)) &&
56 !std::isnan(cloud.normals_[index](1)) &&
57 !std::isnan(cloud.normals_[index](2))) {
58 normal_ += cloud.normals_[index];
59 }
60 }
61 if (cloud.HasColors()) {
62 color_ += cloud.colors_[index];
63 }
64 num_of_points_++;
65 }
66
GetAveragePoint() const67 Eigen::Vector3d GetAveragePoint() const
68 {
69 return point_ / double(num_of_points_);
70 }
71
GetAverageNormal() const72 Eigen::Vector3d GetAverageNormal() const
73 {
74 return normal_.normalized();
75 }
76
GetAverageColor() const77 Eigen::Vector3d GetAverageColor() const
78 {
79 return color_ / double(num_of_points_);
80 }
81
82 private:
83 int num_of_points_;
84 Eigen::Vector3d point_;
85 Eigen::Vector3d normal_;
86 Eigen::Vector3d color_;
87 };
88
89 } // unnamed namespace
90
SelectDownSample(const PointCloud & input,const std::vector<size_t> & indices)91 std::shared_ptr<PointCloud> SelectDownSample(const PointCloud &input,
92 const std::vector<size_t> &indices)
93 {
94 auto output = std::make_shared<PointCloud>();
95 bool has_normals = input.HasNormals();
96 bool has_colors = input.HasColors();
97 for (size_t i : indices) {
98 output->points_.push_back(input.points_[i]);
99 if (has_normals) output->normals_.push_back(input.normals_[i]);
100 if (has_colors) output->colors_.push_back(input.colors_[i]);
101 }
102 PrintDebug("Pointcloud down sampled from %d points to %d points.\n",
103 (int)input.points_.size(), (int)output->points_.size());
104 return output;
105 }
106
SelectDownSample(const TriangleMesh & input,const std::vector<size_t> & indices)107 std::shared_ptr<TriangleMesh> SelectDownSample(const TriangleMesh &input,
108 const std::vector<size_t> &indices)
109 {
110 auto output = std::make_shared<TriangleMesh>();
111 bool has_triangle_normals = input.HasTriangleNormals();
112 bool has_vertex_normals = input.HasVertexNormals();
113 bool has_vertex_colors = input.HasVertexColors();
114 // For each vertex, list face indices.
115 std::vector<std::vector<int>> vertex_to_triangle_temp(input.vertices_.size());
116 int triangle_id = 0;
117 for (auto trangle : input.triangles_) {
118 for (int i=0; i<3; i++)
119 vertex_to_triangle_temp[trangle(i)].push_back(triangle_id);
120 triangle_id++;
121 }
122 // Remove face indices of vertex_to_triangle_temp
123 // if it does not correspond to selected vertices
124 std::vector<std::vector<int>> vertex_to_triangle(input.vertices_.size());
125 for (auto vertex_id : indices) {
126 vertex_to_triangle[vertex_id] = vertex_to_triangle_temp[vertex_id];
127 }
128 // Make a triangle_to_vertex using vertex_to_triangle
129 std::vector<std::vector<int>> triangle_to_vertex(input.triangles_.size());
130 int vertex_id = 0;
131 for (auto face_ids : vertex_to_triangle) {
132 for (auto face_id : face_ids)
133 triangle_to_vertex[face_id].push_back(vertex_id);
134 vertex_id++;
135 }
136 // Only a face with three selected points contributes to mark mask_observed_vertex.
137 std::vector<bool> mask_observed_vertex(input.vertices_.size());
138 for (auto vertex_ids : triangle_to_vertex) {
139 if ((int)vertex_ids.size() == 3)
140 for (int i=0; i<3; i++)
141 mask_observed_vertex[vertex_ids[i]] = true;
142 }
143 // Rename vertex id based on selected points
144 std::vector<int> new_vertex_id(input.vertices_.size());
145 for (auto i=0, cnt=0; i<mask_observed_vertex.size(); i++) {
146 if (mask_observed_vertex[i])
147 new_vertex_id[i] = cnt++;
148 }
149 // Push a triangle that has 3 selected vertices.
150 triangle_id = 0;
151 for (auto vertex_ids : triangle_to_vertex) {
152 if ((int)vertex_ids.size() == 3) {
153 Eigen::Vector3i new_face;
154 for (int i=0; i<3; i++)
155 new_face(i) = new_vertex_id[input.triangles_[triangle_id][i]];
156 output->triangles_.push_back(new_face);
157 if (has_triangle_normals) output->triangle_normals_.push_back(
158 input.triangle_normals_[triangle_id]);
159 }
160 triangle_id++;
161 }
162 // Push marked vertex.
163 for (auto i=0; i<mask_observed_vertex.size(); i++) {
164 if (mask_observed_vertex[i]) {
165 output->vertices_.push_back(input.vertices_[i]);
166 if (has_vertex_normals) output->vertex_normals_.push_back(
167 input.vertex_normals_[i]);
168 if (has_vertex_colors) output->vertex_colors_.push_back(
169 input.vertex_colors_[i]);
170 }
171 }
172 output->Purge();
173 PrintDebug("Triangle mesh sampled from %d vertices and %d triangles to %d vertices and %d triangles.\n",
174 (int)input.vertices_.size(), (int)input.triangles_.size(),
175 (int)output->vertices_.size(), (int)output->triangles_.size());
176 return output;
177 }
178
VoxelDownSample(const PointCloud & input,double voxel_size)179 std::shared_ptr<PointCloud> VoxelDownSample(const PointCloud &input,
180 double voxel_size)
181 {
182 auto output = std::make_shared<PointCloud>();
183 if (voxel_size <= 0.0) {
184 PrintDebug("[VoxelDownSample] voxel_size <= 0.\n");
185 return output;
186 }
187 Eigen::Vector3d voxel_size3 =
188 Eigen::Vector3d(voxel_size, voxel_size, voxel_size);
189 Eigen::Vector3d voxel_min_bound = input.GetMinBound() - voxel_size3 * 0.5;
190 Eigen::Vector3d voxel_max_bound = input.GetMaxBound() + voxel_size3 * 0.5;
191 if (voxel_size * std::numeric_limits<int>::max() <
192 (voxel_max_bound - voxel_min_bound).maxCoeff()) {
193 PrintDebug("[VoxelDownSample] voxel_size is too small.\n");
194 return output;
195 }
196 std::unordered_map<Eigen::Vector3i, AccumulatedPoint,
197 hash_eigen::hash<Eigen::Vector3i>> voxelindex_to_accpoint;
198 Eigen::Vector3d ref_coord;
199 Eigen::Vector3i voxel_index;
200 for (int i = 0; i < (int)input.points_.size(); i++) {
201 ref_coord = (input.points_[i] - voxel_min_bound) / voxel_size;
202 voxel_index << int(floor(ref_coord(0))),
203 int(floor(ref_coord(1))), int(floor(ref_coord(2)));
204 voxelindex_to_accpoint[voxel_index].AddPoint(input, i);
205 }
206 bool has_normals = input.HasNormals();
207 bool has_colors = input.HasColors();
208 for (auto accpoint : voxelindex_to_accpoint) {
209 output->points_.push_back(accpoint.second.GetAveragePoint());
210 if (has_normals) {
211 output->normals_.push_back(accpoint.second.GetAverageNormal());
212 }
213 if (has_colors) {
214 output->colors_.push_back(accpoint.second.GetAverageColor());
215 }
216 }
217 PrintDebug("Pointcloud down sampled from %d points to %d points.\n",
218 (int)input.points_.size(), (int)output->points_.size());
219 return output;
220 }
221
UniformDownSample(const PointCloud & input,size_t every_k_points)222 std::shared_ptr<PointCloud> UniformDownSample(const PointCloud &input,
223 size_t every_k_points)
224 {
225 if (every_k_points == 0) {
226 PrintDebug("[UniformDownSample] Illegal sample rate.\n");
227 return std::make_shared<PointCloud>();
228 }
229 std::vector<size_t> indices;
230 for (size_t i = 0; i < input.points_.size(); i += every_k_points) {
231 indices.push_back(i);
232 }
233 return SelectDownSample(input, indices);
234 }
235
CropPointCloud(const PointCloud & input,const Eigen::Vector3d & min_bound,const Eigen::Vector3d & max_bound)236 std::shared_ptr<PointCloud> CropPointCloud(const PointCloud &input,
237 const Eigen::Vector3d &min_bound, const Eigen::Vector3d &max_bound)
238 {
239 if (min_bound(0) > max_bound(0) || min_bound(1) > max_bound(1) ||
240 min_bound(2) > max_bound(2)) {
241 PrintDebug("[CropPointCloud] Illegal boundary clipped all points.\n");
242 return std::make_shared<PointCloud>();
243 }
244 std::vector<size_t> indices;
245 for (size_t i = 0; i < input.points_.size(); i++) {
246 const auto &point = input.points_[i];
247 if (point(0) >= min_bound(0) && point(0) <= max_bound(0) &&
248 point(1) >= min_bound(1) && point(1) <= max_bound(1) &&
249 point(2) >= min_bound(2) && point(2) <= max_bound(2)) {
250 indices.push_back(i);
251 }
252 }
253 return SelectDownSample(input, indices);
254 }
255
CropTriangleMesh(const TriangleMesh & input,const Eigen::Vector3d & min_bound,const Eigen::Vector3d & max_bound)256 std::shared_ptr<TriangleMesh> CropTriangleMesh(const TriangleMesh &input,
257 const Eigen::Vector3d &min_bound, const Eigen::Vector3d &max_bound)
258 {
259 if (min_bound(0) > max_bound(0) || min_bound(1) > max_bound(1) ||
260 min_bound(2) > max_bound(2)) {
261 PrintDebug("[CropTriangleMesh] Illegal boundary clipped all points.\n");
262 return std::make_shared<TriangleMesh>();
263 }
264 std::vector<size_t> indices;
265 for (size_t i = 0; i < input.vertices_.size(); i++) {
266 const auto &point = input.vertices_[i];
267 if (point(0) >= min_bound(0) && point(0) <= max_bound(0) &&
268 point(1) >= min_bound(1) && point(1) <= max_bound(1) &&
269 point(2) >= min_bound(2) && point(2) <= max_bound(2)) {
270 indices.push_back(i);
271 }
272 }
273 return SelectDownSample(input, indices);
274 }
275
276 } // namespace three
277