1 /*
2 * Software License Agreement (BSD License)
3 *
4 * Point Cloud Library (PCL) - www.pointclouds.org
5 * Copyright (c) 2011, Willow Garage, Inc.
6 *
7 * All rights reserved.
8 *
9 * Redistribution and use in source and binary forms, with or without
10 * modification, are permitted provided that the following conditions
11 * are met:
12 *
13 * * Redistributions of source code must retain the above copyright
14 * notice, this list of conditions and the following disclaimer.
15 * * Redistributions in binary form must reproduce the above
16 * copyright notice, this list of conditions and the following
17 * disclaimer in the documentation and/or other materials provided
18 * with the distribution.
19 * * Neither the name of Willow Garage, Inc. nor the names of its
20 * contributors may be used to endorse or promote products derived
21 * from this software without specific prior written permission.
22 *
23 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
24 * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
25 * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
26 * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
27 * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
28 * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
29 * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
30 * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
31 * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
32 * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
33 * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
34 * POSSIBILITY OF SUCH DAMAGE.
35 *
36 */
37
38 #include <pcl/gpu/kinfu_large_scale/tsdf_volume.h>
39 #include "internal.h"
40 #include <algorithm>
41 #include <Eigen/Core>
42
43 #include <iostream>
44
45 using namespace pcl;
46 using namespace pcl::gpu;
47 using namespace Eigen;
48 using pcl::device::kinfuLS::device_cast;
49
50 ///////////////////////////////////////////////////////////////////////////////////////////////////////////////////
51
TsdfVolume(const Vector3i & resolution)52 pcl::gpu::kinfuLS::TsdfVolume::TsdfVolume(const Vector3i& resolution) : resolution_(resolution), volume_host_ (new std::vector<float>), weights_host_ (new std::vector<short>)
53 {
54 int volume_x = resolution_(0);
55 int volume_y = resolution_(1);
56 int volume_z = resolution_(2);
57
58 volume_.create (volume_y * volume_z, volume_x);
59
60 const Vector3f default_volume_size = Vector3f::Constant (3.f); //meters
61 const float default_tranc_dist = 0.03f; //meters
62
63 setSize(default_volume_size);
64 setTsdfTruncDist(default_tranc_dist);
65
66 reset();
67
68 }
69
70 ///////////////////////////////////////////////////////////////////////////////////////////////////////////////////
71
72 void
setSize(const Vector3f & size)73 pcl::gpu::kinfuLS::TsdfVolume::setSize(const Vector3f& size)
74 {
75 size_ = size;
76 setTsdfTruncDist(tranc_dist_);
77 }
78
79 ///////////////////////////////////////////////////////////////////////////////////////////////////////////////////
80
81 void
setTsdfTruncDist(float distance)82 pcl::gpu::kinfuLS::TsdfVolume::setTsdfTruncDist (float distance)
83 {
84 float cx = size_(0) / resolution_(0);
85 float cy = size_(1) / resolution_(1);
86 float cz = size_(2) / resolution_(2);
87
88 tranc_dist_ = std::max (distance, 2.1f * std::max (cx, std::max (cy, cz)));
89 }
90
91 ///////////////////////////////////////////////////////////////////////////////////////////////////////////////////
92
93 pcl::gpu::DeviceArray2D<int>
data() const94 pcl::gpu::kinfuLS::TsdfVolume::data() const
95 {
96 return volume_;
97 }
98
99 ///////////////////////////////////////////////////////////////////////////////////////////////////////////////////
100
101 const Eigen::Vector3f&
getSize() const102 pcl::gpu::kinfuLS::TsdfVolume::getSize() const
103 {
104 return size_;
105 }
106
107 ///////////////////////////////////////////////////////////////////////////////////////////////////////////////////
108
109 const Eigen::Vector3i&
getResolution() const110 pcl::gpu::kinfuLS::TsdfVolume::getResolution() const
111 {
112 return resolution_;
113 }
114
115 ///////////////////////////////////////////////////////////////////////////////////////////////////////////////////
116
117 const Eigen::Vector3f
getVoxelSize() const118 pcl::gpu::kinfuLS::TsdfVolume::getVoxelSize() const
119 {
120 return size_.array () / resolution_.array().cast<float>();
121 }
122
123 ///////////////////////////////////////////////////////////////////////////////////////////////////////////////////
124
125 float
getTsdfTruncDist() const126 pcl::gpu::kinfuLS::TsdfVolume::getTsdfTruncDist () const
127 {
128 return tranc_dist_;
129 }
130
131 ///////////////////////////////////////////////////////////////////////////////////////////////////////////////////
132
133 void
reset()134 pcl::gpu::kinfuLS::TsdfVolume::reset()
135 {
136 pcl::device::kinfuLS::initVolume(volume_);
137 }
138
139 ///////////////////////////////////////////////////////////////////////////////////////////////////////////////////
140
141 void
fetchCloudHost(PointCloud<PointXYZI> & cloud,bool connected26) const142 pcl::gpu::kinfuLS::TsdfVolume::fetchCloudHost (PointCloud<PointXYZI>& cloud, bool connected26) const
143 {
144 PointCloud<PointXYZ>::Ptr cloud_ptr_ = PointCloud<PointXYZ>::Ptr (new PointCloud<PointXYZ>);
145 PointCloud<PointIntensity>::Ptr cloud_i_ptr_ = PointCloud<PointIntensity>::Ptr (new PointCloud<PointIntensity>);
146 fetchCloudHost(*cloud_ptr_);
147 pcl::concatenateFields (*cloud_ptr_, *cloud_i_ptr_, cloud);
148 }
149
150 ///////////////////////////////////////////////////////////////////////////////////////////////////////////////////
151
152 void
fetchCloudHost(PointCloud<PointType> & cloud,bool connected26) const153 pcl::gpu::kinfuLS::TsdfVolume::fetchCloudHost (PointCloud<PointType>& cloud, bool connected26) const
154 {
155 int volume_x = resolution_(0);
156 int volume_y = resolution_(1);
157 int volume_z = resolution_(2);
158
159 int cols;
160 std::vector<int> volume_host;
161 volume_.download (volume_host, cols);
162
163 cloud.clear ();
164 cloud.reserve (10000);
165
166 constexpr int DIVISOR = pcl::device::kinfuLS::DIVISOR; // SHRT_MAX;
167
168 #define FETCH(x, y, z) volume_host[(x) + (y) * volume_x + (z) * volume_y * volume_x]
169
170 Array3f cell_size = getVoxelSize();
171
172 for (int x = 1; x < volume_x-1; ++x)
173 {
174 for (int y = 1; y < volume_y-1; ++y)
175 {
176 for (int z = 0; z < volume_z-1; ++z)
177 {
178 int tmp = FETCH (x, y, z);
179 int W = reinterpret_cast<short2*>(&tmp)->y;
180 int F = reinterpret_cast<short2*>(&tmp)->x;
181
182 if (W == 0 || F == DIVISOR)
183 continue;
184
185 Vector3f V = ((Array3f(x, y, z) + 0.5f) * cell_size).matrix ();
186
187 if (connected26)
188 {
189 int dz = 1;
190 for (int dy = -1; dy < 2; ++dy)
191 for (int dx = -1; dx < 2; ++dx)
192 {
193 int tmp = FETCH (x+dx, y+dy, z+dz);
194
195 int Wn = reinterpret_cast<short2*>(&tmp)->y;
196 int Fn = reinterpret_cast<short2*>(&tmp)->x;
197 if (Wn == 0 || Fn == DIVISOR)
198 continue;
199
200 if ((F > 0 && Fn < 0) || (F < 0 && Fn > 0))
201 {
202 Vector3f Vn = ((Array3f (x+dx, y+dy, z+dz) + 0.5f) * cell_size).matrix ();
203 Vector3f point = (V * std::abs (Fn) + Vn * std::abs (F)) / (std::abs (F) + std::abs (Fn));
204
205 pcl::PointXYZ xyz;
206 xyz.x = point (0);
207 xyz.y = point (1);
208 xyz.z = point (2);
209
210 cloud.push_back (xyz);
211 }
212 }
213 dz = 0;
214 for (int dy = 0; dy < 2; ++dy)
215 for (int dx = -1; dx < dy * 2; ++dx)
216 {
217 int tmp = FETCH (x+dx, y+dy, z+dz);
218
219 int Wn = reinterpret_cast<short2*>(&tmp)->y;
220 int Fn = reinterpret_cast<short2*>(&tmp)->x;
221 if (Wn == 0 || Fn == DIVISOR)
222 continue;
223
224 if ((F > 0 && Fn < 0) || (F < 0 && Fn > 0))
225 {
226 Vector3f Vn = ((Array3f (x+dx, y+dy, z+dz) + 0.5f) * cell_size).matrix ();
227 Vector3f point = (V * std::abs(Fn) + Vn * std::abs(F))/(std::abs(F) + std::abs (Fn));
228
229 pcl::PointXYZ xyz;
230 xyz.x = point (0);
231 xyz.y = point (1);
232 xyz.z = point (2);
233
234 cloud.push_back (xyz);
235 }
236 }
237 }
238 else /* if (connected26) */
239 {
240 for (int i = 0; i < 3; ++i)
241 {
242 int ds[] = {0, 0, 0};
243 ds[i] = 1;
244
245 int dx = ds[0];
246 int dy = ds[1];
247 int dz = ds[2];
248
249 int tmp = FETCH (x+dx, y+dy, z+dz);
250
251 int Wn = reinterpret_cast<short2*>(&tmp)->y;
252 int Fn = reinterpret_cast<short2*>(&tmp)->x;
253 if (Wn == 0 || Fn == DIVISOR)
254 continue;
255
256 if ((F > 0 && Fn < 0) || (F < 0 && Fn > 0))
257 {
258 Vector3f Vn = ((Array3f (x+dx, y+dy, z+dz) + 0.5f) * cell_size).matrix ();
259 Vector3f point = (V * std::abs (Fn) + Vn * std::abs (F)) / (std::abs (F) + std::abs (Fn));
260
261 pcl::PointXYZ xyz;
262 xyz.x = point (0);
263 xyz.y = point (1);
264 xyz.z = point (2);
265
266 cloud.push_back (xyz);
267 }
268 }
269 } /* if (connected26) */
270 }
271 }
272 }
273 #undef FETCH
274 cloud.width = cloud.size ();
275 cloud.height = 1;
276 }
277
278 ///////////////////////////////////////////////////////////////////////////////////////////////////////////////////
279 pcl::gpu::DeviceArray<pcl::gpu::kinfuLS::TsdfVolume::PointType>
fetchCloud(DeviceArray<PointType> & cloud_buffer) const280 pcl::gpu::kinfuLS::TsdfVolume::fetchCloud (DeviceArray<PointType>& cloud_buffer) const
281 {
282 if (cloud_buffer.empty ())
283 cloud_buffer.create (DEFAULT_CLOUD_BUFFER_SIZE);
284
285 float3 device_volume_size = device_cast<const float3> (size_);
286 std::size_t size = pcl::device::kinfuLS::extractCloud (volume_, device_volume_size, cloud_buffer);
287 return (DeviceArray<PointType> (cloud_buffer.ptr (), size));
288 }
289
290 ///////////////////////////////////////////////////////////////////////////////////////////////////////////////////
291
292 void
fetchNormals(const DeviceArray<PointType> & cloud,DeviceArray<PointType> & normals) const293 pcl::gpu::kinfuLS::TsdfVolume::fetchNormals (const DeviceArray<PointType>& cloud, DeviceArray<PointType>& normals) const
294 {
295 normals.create (cloud.size ());
296 const float3 device_volume_size = device_cast<const float3> (size_);
297 pcl::device::kinfuLS::extractNormals (volume_, device_volume_size, cloud, (pcl::device::kinfuLS::PointType*)normals.ptr ());
298 }
299
300 ///////////////////////////////////////////////////////////////////////////////////////////////////////////////////
301
302 void
pushSlice(PointCloud<PointXYZI>::Ptr existing_data_cloud,const pcl::gpu::kinfuLS::tsdf_buffer * buffer) const303 pcl::gpu::kinfuLS::TsdfVolume::pushSlice (PointCloud<PointXYZI>::Ptr existing_data_cloud, const pcl::gpu::kinfuLS::tsdf_buffer* buffer) const
304 {
305 const auto gpu_array_size = existing_data_cloud->size ();
306
307 if(gpu_array_size == 0)
308 {
309 //std::cout << "[KinfuTracker](pushSlice) Existing data cloud has no points\n";//CREATE AS PCL MESSAGE
310 return;
311 }
312
313 const pcl::PointXYZI *first_point_ptr = &((*existing_data_cloud)[0]);
314
315 pcl::gpu::DeviceArray<pcl::PointXYZI> cloud_gpu;
316 cloud_gpu.upload (first_point_ptr, gpu_array_size);
317
318 DeviceArray<float4>& cloud_cast = (DeviceArray<float4>&) cloud_gpu;
319 //volume().pushCloudAsSlice (cloud_cast, &buffer_);
320 pcl::device::kinfuLS::pushCloudAsSliceGPU (volume_, cloud_cast, buffer);
321 }
322
323 ///////////////////////////////////////////////////////////////////////////////////////////////////////////////////
324
325 size_t
fetchSliceAsCloud(DeviceArray<PointType> & cloud_buffer_xyz,DeviceArray<float> & cloud_buffer_intensity,const pcl::gpu::kinfuLS::tsdf_buffer * buffer,int shiftX,int shiftY,int shiftZ) const326 pcl::gpu::kinfuLS::TsdfVolume::fetchSliceAsCloud (DeviceArray<PointType>& cloud_buffer_xyz, DeviceArray<float>& cloud_buffer_intensity, const pcl::gpu::kinfuLS::tsdf_buffer* buffer, int shiftX, int shiftY, int shiftZ ) const
327 {
328 if (cloud_buffer_xyz.empty ())
329 cloud_buffer_xyz.create (DEFAULT_CLOUD_BUFFER_SIZE/2);
330
331 if (cloud_buffer_intensity.empty ()) {
332 cloud_buffer_intensity.create (DEFAULT_CLOUD_BUFFER_SIZE/2);
333 }
334
335 float3 device_volume_size = device_cast<const float3> (size_);
336
337 std::size_t size = pcl::device::kinfuLS::extractSliceAsCloud (volume_, device_volume_size, buffer, shiftX, shiftY, shiftZ, cloud_buffer_xyz, cloud_buffer_intensity);
338
339 std::cout << " SIZE IS " << size << std::endl;
340
341 return (size);
342 }
343
344 ///////////////////////////////////////////////////////////////////////////////////////////////////////////////////
345
346 void
fetchNormals(const DeviceArray<PointType> & cloud,DeviceArray<NormalType> & normals) const347 pcl::gpu::kinfuLS::TsdfVolume::fetchNormals (const DeviceArray<PointType>& cloud, DeviceArray<NormalType>& normals) const
348 {
349 normals.create (cloud.size ());
350 const float3 device_volume_size = device_cast<const float3> (size_);
351 pcl::device::kinfuLS::extractNormals (volume_, device_volume_size, cloud, (pcl::device::kinfuLS::float8*)normals.ptr ());
352 }
353
354 ///////////////////////////////////////////////////////////////////////////////////////////////////////////////////
355
356 void
convertToTsdfCloud(pcl::PointCloud<pcl::PointXYZI>::Ptr & cloud,const unsigned step) const357 pcl::gpu::kinfuLS::TsdfVolume::convertToTsdfCloud ( pcl::PointCloud<pcl::PointXYZI>::Ptr &cloud,
358 const unsigned step) const
359 {
360 int sx = header_.resolution(0);
361 int sy = header_.resolution(1);
362 int sz = header_.resolution(2);
363
364 const int cloud_size = static_cast<int> (header_.getVolumeSize() / (step*step*step));
365
366 cloud->clear();
367 cloud->reserve (std::min (cloud_size/10, 500000));
368
369 int volume_idx = 0, cloud_idx = 0;
370 for (int z = 0; z < sz; z+=step)
371 for (int y = 0; y < sy; y+=step)
372 for (int x = 0; x < sx; x+=step, ++cloud_idx)
373 {
374 volume_idx = sx*sy*z + sx*y + x;
375 // pcl::PointXYZI &point = (*cloud)[cloud_idx];
376
377 if (weights_host_->at(volume_idx) == 0 || volume_host_->at(volume_idx) > 0.98 )
378 continue;
379
380 pcl::PointXYZI point;
381 point.x = x; point.y = y; point.z = z;//*64;
382 point.intensity = volume_host_->at(volume_idx);
383 cloud->push_back (point);
384 }
385 }
386
387 ///////////////////////////////////////////////////////////////////////////////////////////////////////////////////
388
389 void
downloadTsdf(std::vector<float> & tsdf) const390 pcl::gpu::kinfuLS::TsdfVolume::downloadTsdf (std::vector<float>& tsdf) const
391 {
392 tsdf.resize (volume_.cols() * volume_.rows());
393 volume_.download(&tsdf[0], volume_.cols() * sizeof(int));
394
395 #pragma omp parallel for \
396 default(none) \
397 shared(tsdf)
398 for(int i = 0; i < (int) tsdf.size(); ++i)
399 {
400 float tmp = reinterpret_cast<short2*>(&tsdf[i])->x;
401 tsdf[i] = tmp/pcl::device::kinfuLS::DIVISOR;
402 }
403 }
404
405 void
downloadTsdfLocal() const406 pcl::gpu::kinfuLS::TsdfVolume::downloadTsdfLocal () const
407 {
408 pcl::gpu::kinfuLS::TsdfVolume::downloadTsdf (*volume_host_);
409 }
410
411
412 ///////////////////////////////////////////////////////////////////////////////////////////////////////////////////
413
414 void
downloadTsdfAndWeights(std::vector<float> & tsdf,std::vector<short> & weights) const415 pcl::gpu::kinfuLS::TsdfVolume::downloadTsdfAndWeights (std::vector<float>& tsdf, std::vector<short>& weights) const
416 {
417 int volumeSize = volume_.cols() * volume_.rows();
418 tsdf.resize (volumeSize);
419 weights.resize (volumeSize);
420 volume_.download(&tsdf[0], volume_.cols() * sizeof(int));
421
422 #pragma omp parallel for \
423 default(none) \
424 shared(tsdf, weights)
425 for(int i = 0; i < (int) tsdf.size(); ++i)
426 {
427 short2 elem = *reinterpret_cast<short2*>(&tsdf[i]);
428 tsdf[i] = (float)(elem.x)/pcl::device::kinfuLS::DIVISOR;
429 weights[i] = (short)(elem.y);
430 }
431 }
432
433
434 void
downloadTsdfAndWeightsLocal() const435 pcl::gpu::kinfuLS::TsdfVolume::downloadTsdfAndWeightsLocal () const
436 {
437 pcl::gpu::kinfuLS::TsdfVolume::downloadTsdfAndWeights (*volume_host_, *weights_host_);
438 }
439
440 ///////////////////////////////////////////////////////////////////////////////////////////////////////////////////
441
442 bool
save(const std::string & filename,bool binary) const443 pcl::gpu::kinfuLS::TsdfVolume::save (const std::string &filename, bool binary) const
444 {
445 pcl::console::print_info ("Saving TSDF volume to "); pcl::console::print_value ("%s ... ", filename.c_str());
446 std::cout << std::flush;
447
448 std::ofstream file (filename.c_str(), binary ? std::ios_base::binary : std::ios_base::out);
449
450 if (file.is_open())
451 {
452 if (binary)
453 {
454 // HEADER
455 // write resolution and size of volume
456 file.write ((char*) &header_, sizeof (Header));
457 /* file.write ((char*) &header_.resolution, sizeof(Eigen::Vector3i));
458 file.write ((char*) &header_.volume_size, sizeof(Eigen::Vector3f));
459 // write element size
460 int volume_element_size = sizeof(VolumeT);
461 file.write ((char*) &volume_element_size, sizeof(int));
462 int weights_element_size = sizeof(WeightT);
463 file.write ((char*) &weights_element_size, sizeof(int)); */
464
465 // DATA
466 // write data
467 file.write ((char*) &(volume_host_->at(0)), volume_host_->size()*sizeof(float));
468 file.write ((char*) &(weights_host_->at(0)), weights_host_->size()*sizeof(short));
469 }
470 else
471 {
472 // write resolution and size of volume and element size
473 file << header_.resolution(0) << " " << header_.resolution(1) << " " << header_.resolution(2) << std::endl;
474 file << header_.volume_size(0) << " " << header_.volume_size(1) << " " << header_.volume_size(2) << std::endl;
475 file << sizeof (float) << " " << sizeof(short) << std::endl;
476
477 // write data
478 for (std::vector<float>::const_iterator iter = volume_host_->begin(); iter != volume_host_->end(); ++iter)
479 file << *iter << std::endl;
480 }
481
482 file.close();
483 }
484 else
485 {
486 pcl::console::print_error ("[saveTsdfVolume] Error: Couldn't open file %s.\n", filename.c_str());
487 return false;
488 }
489
490 pcl::console::print_info ("done [%d voxels]\n", this->size ());
491
492 return true;
493 }
494
495
496 bool
load(const std::string & filename,bool binary)497 pcl::gpu::kinfuLS::TsdfVolume::load (const std::string &filename, bool binary)
498 {
499 pcl::console::print_info ("Loading TSDF volume from "); pcl::console::print_value ("%s ... ", filename.c_str());
500 std::cout << std::flush;
501
502 std::ifstream file (filename.c_str());
503
504 if (file.is_open())
505 {
506 if (binary)
507 {
508 // read HEADER
509 file.read ((char*) &header_, sizeof (Header));
510 /* file.read (&header_.resolution, sizeof(Eigen::Array3i));
511 file.read (&header_.volume_size, sizeof(Eigen::Vector3f));
512 file.read (&header_.volume_element_size, sizeof(int));
513 file.read (&header_.weights_element_size, sizeof(int)); */
514
515 // check if element size fits to data
516 if (header_.volume_element_size != sizeof(float))
517 {
518 pcl::console::print_error ("[TSDFVolume::load] Error: Given volume element size (%d) doesn't fit data (%d)", sizeof(float), header_.volume_element_size);
519 return false;
520 }
521 if ( header_.weights_element_size != sizeof(short))
522 {
523 pcl::console::print_error ("[TSDFVolume::load] Error: Given weights element size (%d) doesn't fit data (%d)", sizeof(short), header_.weights_element_size);
524 return false;
525 }
526
527 // read DATA
528 int num_elements = header_.getVolumeSize();
529 volume_host_->resize (num_elements);
530 weights_host_->resize (num_elements);
531 file.read ((char*) &(*volume_host_)[0], num_elements * sizeof(float));
532 file.read ((char*) &(*weights_host_)[0], num_elements * sizeof(short));
533 }
534 else
535 {
536 pcl::console::print_error ("[TSDFVolume::load] Error: ASCII loading not implemented.\n");
537 }
538
539 file.close ();
540 }
541 else
542 {
543 pcl::console::print_error ("[TSDFVolume::load] Error: Cloudn't read file %s.\n", filename.c_str());
544 return false;
545 }
546
547 const Eigen::Vector3i &res = this->gridResolution();
548 pcl::console::print_info ("done [%d voxels, res %dx%dx%d]\n", this->size(), res[0], res[1], res[2]);
549
550 return true;
551 }
552