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 "ScalableTSDFVolume.h"
28
29 #include <unordered_set>
30
31 #include <Core/Utility/Console.h>
32 #include <Core/Geometry/PointCloud.h>
33 #include <Core/Integration/UniformTSDFVolume.h>
34 #include <Core/Integration/MarchingCubesConst.h>
35
36 namespace three{
37
ScalableTSDFVolume(double voxel_length,double sdf_trunc,bool with_color,int volume_unit_resolution,int depth_sampling_stride)38 ScalableTSDFVolume::ScalableTSDFVolume(double voxel_length, double sdf_trunc,
39 bool with_color, int volume_unit_resolution/* = 16*/,
40 int depth_sampling_stride/* = 4*/) :
41 TSDFVolume(voxel_length, sdf_trunc, with_color),
42 volume_unit_resolution_(volume_unit_resolution),
43 volume_unit_length_(voxel_length * volume_unit_resolution),
44 depth_sampling_stride_(depth_sampling_stride)
45 {
46 }
47
~ScalableTSDFVolume()48 ScalableTSDFVolume::~ScalableTSDFVolume()
49 {
50 }
51
Reset()52 void ScalableTSDFVolume::Reset()
53 {
54 volume_units_.clear();
55 }
56
Integrate(const RGBDImage & image,const PinholeCameraIntrinsic & intrinsic,const Eigen::Matrix4d & extrinsic)57 void ScalableTSDFVolume::Integrate(const RGBDImage &image,
58 const PinholeCameraIntrinsic &intrinsic,
59 const Eigen::Matrix4d &extrinsic)
60 {
61 if ((image.depth_.num_of_channels_ != 1) ||
62 (image.depth_.bytes_per_channel_ != 4) ||
63 (image.depth_.width_ != intrinsic.width_) ||
64 (image.depth_.height_ != intrinsic.height_) ||
65 (with_color_ && image.color_.num_of_channels_ != 3) ||
66 (with_color_ && image.color_.bytes_per_channel_ != 1) ||
67 (with_color_ && image.color_.width_ != intrinsic.width_) ||
68 (with_color_ && image.color_.height_ != intrinsic.height_)) {
69 PrintWarning("[ScalableTSDFVolume::Integrate] Unsupported image format. Please check if you have called CreateRGBDImageFromColorAndDepth() with convert_rgb_to_intensity=false.\n");
70 return;
71 }
72 auto depth2cameradistance = CreateDepthToCameraDistanceMultiplierFloatImage(
73 intrinsic);
74 auto pointcloud = CreatePointCloudFromDepthImage(image.depth_, intrinsic,
75 extrinsic, 1000.0, 1000.0, depth_sampling_stride_);
76 std::unordered_set<Eigen::Vector3i, hash_eigen::hash<Eigen::Vector3i>>
77 touched_volume_units_;
78 for (const auto &point : pointcloud->points_) {
79 auto min_bound = LocateVolumeUnit(point - Eigen::Vector3d(
80 sdf_trunc_, sdf_trunc_, sdf_trunc_));
81 auto max_bound = LocateVolumeUnit(point + Eigen::Vector3d(
82 sdf_trunc_, sdf_trunc_, sdf_trunc_));
83 for (auto x = min_bound(0); x <= max_bound(0); x++) {
84 for (auto y = min_bound(1); y <= max_bound(1); y++) {
85 for (auto z = min_bound(2); z <= max_bound(2); z++) {
86 auto loc = Eigen::Vector3i(x, y, z);
87 if (touched_volume_units_.find(loc) ==
88 touched_volume_units_.end()) {
89 touched_volume_units_.insert(loc);
90 auto volume = OpenVolumeUnit(Eigen::Vector3i(x, y, z));
91 volume->IntegrateWithDepthToCameraDistanceMultiplier(
92 image, intrinsic, extrinsic,
93 *depth2cameradistance);
94 }
95 }
96 }
97 }
98 }
99 }
100
ExtractPointCloud()101 std::shared_ptr<PointCloud> ScalableTSDFVolume::ExtractPointCloud()
102 {
103 auto pointcloud = std::make_shared<PointCloud>();
104 double half_voxel_length = voxel_length_ * 0.5;
105 float w0, w1, f0, f1;
106 Eigen::Vector3f c0, c1;
107 for (const auto &unit : volume_units_) {
108 if (unit.second.volume_) {
109 const auto &volume0 = *unit.second.volume_;
110 const auto &index0 = unit.second.index_;
111 for (int x = 0; x < volume0.resolution_; x++) {
112 for (int y = 0; y < volume0.resolution_; y++) {
113 for (int z = 0; z < volume0.resolution_; z++) {
114 Eigen::Vector3i idx0(x, y, z);
115 w0 = volume0.weight_[volume0.IndexOf(idx0)];
116 f0 = volume0.tsdf_[volume0.IndexOf(idx0)];
117 if (with_color_)
118 c0 = volume0.color_[volume0.IndexOf(idx0)];
119 if (w0 != 0.0f && f0 < 0.98f && f0 >= -0.98f) {
120 Eigen::Vector3d p0 = Eigen::Vector3d(
121 half_voxel_length + voxel_length_ * x,
122 half_voxel_length + voxel_length_ * y,
123 half_voxel_length + voxel_length_ * z) +
124 index0.cast<double>() * volume_unit_length_;
125 for (int i = 0; i < 3; i++) {
126 Eigen::Vector3d p1 = p0;
127 Eigen::Vector3i idx1 = idx0;
128 Eigen::Vector3i index1 = index0;
129 p1(i) += voxel_length_;
130 idx1(i) += 1;
131 if (idx1(i) < volume0.resolution_) {
132 w1 = volume0.weight_[volume0.IndexOf(idx1)];
133 f1 = volume0.tsdf_[volume0.IndexOf(idx1)];
134 if (with_color_)
135 c1 = volume0.color_[
136 volume0.IndexOf(idx1)];
137 } else {
138 idx1(i) -= volume0.resolution_;
139 index1(i) += 1;
140 auto unit_itr = volume_units_.find(index1);
141 if (unit_itr == volume_units_.end()) {
142 w1 = 0.0f; f1 = 0.0f;
143 } else {
144 const auto &volume1 =
145 *unit_itr->second.volume_;
146 w1 = volume1.weight_[volume1.IndexOf(
147 idx1)];
148 f1 = volume1.tsdf_[volume1.IndexOf(
149 idx1)];
150 if (with_color_)
151 c1 = volume1.color_[
152 volume1.IndexOf(idx1)];
153 }
154 }
155 if (w1 != 0.0f && f1 < 0.98f && f1 >= -0.98f &&
156 f0 * f1 < 0) {
157 float r0 = std::fabs(f0);
158 float r1 = std::fabs(f1);
159 Eigen::Vector3d p = p0;
160 p(i) = (p0(i) * r1 + p1(i) * r0) /
161 (r0 + r1);
162 pointcloud->points_.push_back(p);
163 if (with_color_) {
164 pointcloud->colors_.push_back(
165 ((c0 * r1 + c1 * r0) /
166 (r0 + r1) / 255.0f).
167 cast<double>());
168 }
169 // has_normal
170 pointcloud->normals_.push_back(
171 GetNormalAt(p));
172 }
173 }
174 }
175 }
176 }
177 }
178 }
179 }
180 return pointcloud;
181 }
182
ExtractTriangleMesh()183 std::shared_ptr<TriangleMesh> ScalableTSDFVolume::ExtractTriangleMesh()
184 {
185 // implementation of marching cubes, based on
186 // http://paulbourke.net/geometry/polygonise/
187 auto mesh = std::make_shared<TriangleMesh>();
188 double half_voxel_length = voxel_length_ * 0.5;
189 std::unordered_map<Eigen::Vector4i, int, hash_eigen::hash<Eigen::Vector4i>>
190 edgeindex_to_vertexindex;
191 int edge_to_index[12];
192 for (const auto &unit : volume_units_) {
193 if (unit.second.volume_) {
194 const auto &volume0 = *unit.second.volume_;
195 const auto &index0 = unit.second.index_;
196 for (int x = 0; x < volume0.resolution_; x++) {
197 for (int y = 0; y < volume0.resolution_; y++) {
198 for (int z = 0; z < volume0.resolution_; z++) {
199 Eigen::Vector3i idx0(x, y, z);
200 int cube_index = 0;
201 float w[8];
202 float f[8];
203 Eigen::Vector3d c[8];
204 for (int i = 0; i < 8; i++) {
205 Eigen::Vector3i index1 = index0;
206 Eigen::Vector3i idx1 = idx0 + shift[i];
207 if (idx1(0) < volume_unit_resolution_ &&
208 idx1(1) < volume_unit_resolution_ &&
209 idx1(2) < volume_unit_resolution_) {
210 w[i] = volume0.weight_[volume0.IndexOf(idx1)];
211 f[i] = volume0.tsdf_[volume0.IndexOf(idx1)];
212 if (with_color_)
213 c[i] = volume0.color_[volume0.IndexOf(
214 idx1)].cast<double>() / 255.0;;
215 } else {
216 for (int j = 0; j < 3; j++) {
217 if (idx1(j) >= volume_unit_resolution_) {
218 idx1(j) -= volume_unit_resolution_;
219 index1(j) += 1;
220 }
221 }
222 auto unit_itr1 = volume_units_.find(index1);
223 if (unit_itr1 == volume_units_.end()) {
224 w[i] = 0.0f; f[i] = 0.0f;
225 } else {
226 const auto &volume1 =
227 *unit_itr1->second.volume_;
228 w[i] = volume1.weight_[volume1.IndexOf(
229 idx1)];
230 f[i] = volume1.tsdf_[volume1.IndexOf(idx1)];
231 if (with_color_)
232 c[i] = volume1.color_[volume1.IndexOf(
233 idx1)].cast<double>() / 255.0;
234 }
235 }
236 if (w[i] == 0.0f) {
237 cube_index = 0;
238 break;
239 } else {
240 if (f[i] < 0.0f) {
241 cube_index |= (1 << i);
242 }
243 }
244 }
245 if (cube_index == 0 || cube_index == 255) {
246 continue;
247 }
248 for (int i = 0; i < 12; i++) {
249 if (edge_table[cube_index] & (1 << i)) {
250 Eigen::Vector4i edge_index = Eigen::Vector4i(
251 index0(0), index0(1), index0(2), 0) *
252 volume_unit_resolution_ +
253 Eigen::Vector4i(x, y, z, 0) +
254 edge_shift[i];
255 if (edgeindex_to_vertexindex.find(edge_index) ==
256 edgeindex_to_vertexindex.end()) {
257 edge_to_index[i] =
258 (int)mesh->vertices_.size();
259 edgeindex_to_vertexindex[edge_index] =
260 (int)mesh->vertices_.size();
261 Eigen::Vector3d pt(
262 half_voxel_length +
263 voxel_length_ * edge_index(0),
264 half_voxel_length +
265 voxel_length_ * edge_index(1),
266 half_voxel_length +
267 voxel_length_ * edge_index(2));
268 double f0 = std::abs((double)f[
269 edge_to_vert[i][0]]);
270 double f1 = std::abs((double)f[
271 edge_to_vert[i][1]]);
272 pt(edge_index(3)) += f0 * voxel_length_ /
273 (f0 + f1);
274 mesh->vertices_.push_back(pt);
275 if (with_color_) {
276 const auto &c0 = c[edge_to_vert[i][0]];
277 const auto &c1 = c[edge_to_vert[i][1]];
278 mesh->vertex_colors_.push_back(
279 (f1 * c0 + f0 * c1) / (f0 + f1));
280 }
281 } else {
282 edge_to_index[i] =
283 edgeindex_to_vertexindex[
284 edge_index];
285 }
286 }
287 }
288 for (int i = 0; tri_table[cube_index][i] != -1; i += 3)
289 {
290 mesh->triangles_.push_back(Eigen::Vector3i(
291 edge_to_index[tri_table[cube_index][i]],
292 edge_to_index[tri_table[cube_index][i + 2]],
293 edge_to_index[tri_table[cube_index][i + 1]]));
294 }
295 }
296 }
297 }
298 }
299 }
300 return mesh;
301 }
302
ExtractVoxelPointCloud()303 std::shared_ptr<PointCloud> ScalableTSDFVolume::ExtractVoxelPointCloud()
304 {
305 auto voxel = std::make_shared<PointCloud>();
306 for (auto &unit : volume_units_) {
307 if (unit.second.volume_) {
308 auto v = unit.second.volume_->ExtractVoxelPointCloud();
309 *voxel += *v;
310 }
311 }
312 return voxel;
313 }
314
OpenVolumeUnit(const Eigen::Vector3i & index)315 std::shared_ptr<UniformTSDFVolume> ScalableTSDFVolume::OpenVolumeUnit(
316 const Eigen::Vector3i &index)
317 {
318 auto &unit = volume_units_[index];
319 if (!unit.volume_) {
320 unit.volume_.reset(new UniformTSDFVolume(volume_unit_length_,
321 volume_unit_resolution_, sdf_trunc_, with_color_,
322 index.cast<double>() * volume_unit_length_));
323 unit.index_ = index;
324 }
325 return unit.volume_;
326 }
327
GetNormalAt(const Eigen::Vector3d & p)328 Eigen::Vector3d ScalableTSDFVolume::GetNormalAt(const Eigen::Vector3d &p)
329 {
330 Eigen::Vector3d n;
331 const double half_gap = 0.99 * voxel_length_;
332 for (int i = 0; i < 3; i++) {
333 Eigen::Vector3d p0 = p;
334 p0(i) -= half_gap;
335 Eigen::Vector3d p1 = p;
336 p1(i) += half_gap;
337 n(i) = GetTSDFAt(p1) - GetTSDFAt(p0);
338 }
339 return n.normalized();
340 }
341
GetTSDFAt(const Eigen::Vector3d & p)342 double ScalableTSDFVolume::GetTSDFAt(const Eigen::Vector3d &p)
343 {
344 Eigen::Vector3d p_locate = p - Eigen::Vector3d(0.5, 0.5, 0.5) *
345 voxel_length_;
346 Eigen::Vector3i index0 = LocateVolumeUnit(p_locate);
347 auto unit_itr = volume_units_.find(index0);
348 if (unit_itr == volume_units_.end()) {
349 return 0.0;
350 }
351 const auto &volume0 = *unit_itr->second.volume_;
352 Eigen::Vector3i idx0;
353 Eigen::Vector3d p_grid = (p_locate - index0.cast<double>() *
354 volume_unit_length_) / voxel_length_;
355 for (int i = 0; i < 3; i++) {
356 idx0(i) = (int)std::floor(p_grid(i));
357 if (idx0(i) < 0) idx0(i) = 0;
358 if (idx0(i) >= volume_unit_resolution_)
359 idx0(i) = volume_unit_resolution_ - 1;
360 }
361 Eigen::Vector3d r = p_grid - idx0.cast<double>();
362 float f[8];
363 for (int i = 0; i < 8; i++) {
364 Eigen::Vector3i index1 = index0;
365 Eigen::Vector3i idx1 = idx0 + shift[i];
366 if (idx1(0) < volume_unit_resolution_ &&
367 idx1(1) < volume_unit_resolution_ &&
368 idx1(2) < volume_unit_resolution_) {
369 f[i] = volume0.tsdf_[volume0.IndexOf(idx1)];
370 } else {
371 for (int j = 0; j < 3; j++) {
372 if (idx1(j) >= volume_unit_resolution_) {
373 idx1(j) -= volume_unit_resolution_;
374 index1(j) += 1;
375 }
376 }
377 auto unit_itr1 = volume_units_.find(index1);
378 if (unit_itr1 == volume_units_.end()) {
379 f[i] = 0.0f;
380 } else {
381 const auto &volume1 = *unit_itr1->second.volume_;
382 f[i] = volume1.tsdf_[volume1.IndexOf(idx1)];
383 }
384 }
385 }
386 return (1 - r(0)) * ( (1 - r(1)) * ((1 - r(2)) * f[0] + r(2) * f[4]) +
387 r(1) * ((1 - r(2)) * f[3] + r(2) * f[7])) +
388 r(0) * ((1 - r(1)) * ((1 - r(2)) * f[1] + r(2) * f[5]) +
389 r(1) * ((1 - r(2)) * f[2] + r(2) * f[6]));
390 }
391
392 } // namespace three
393