40 #ifndef PCL_OUTOFCORE_OCTREE_DISK_CONTAINER_IMPL_H_
41 #define PCL_OUTOFCORE_OCTREE_DISK_CONTAINER_IMPL_H_
49 #include <pcl/outofcore/boost.h>
52 #include <pcl/io/pcd_io.h>
53 #include <pcl/point_types.h>
54 #include <pcl/PCLPointCloud2.h>
57 #include <pcl/outofcore/octree_disk_container.h>
61 #define _fseeki64 fseeko
62 #elif defined __MINGW32__
63 #define _fseeki64 fseeko64
70 template<
typename Po
intT>
71 boost::mutex OutofcoreOctreeDiskContainer<PointT>::rng_mutex_;
73 template<
typename Po
intT> boost::mt19937
74 OutofcoreOctreeDiskContainer<PointT>::rand_gen_ (static_cast<unsigned int> (std::time(NULL)));
76 template<
typename Po
intT>
79 template<
typename Po
intT>
81 template<
typename Po
intT>
84 template<
typename Po
intT>
void
89 boost::mutex::scoped_lock lock (rng_mutex_);
99 template<
typename Po
intT>
101 : disk_storage_filename_ ()
107 disk_storage_filename_ = boost::shared_ptr<std::string> (
new std::string (temp));
112 template<
typename Po
intT>
114 : disk_storage_filename_ ()
118 if (boost::filesystem::exists (path))
120 if (boost::filesystem::is_directory (path))
124 boost::filesystem::path filename (uuid);
125 boost::filesystem::path file = path / filename;
127 disk_storage_filename_ = boost::shared_ptr<std::string> (
new std::string (file.string ()));
131 uint64_t len = boost::filesystem::file_size (path);
133 disk_storage_filename_ = boost::shared_ptr<std::string> (
new std::string (path.string ()));
135 filelen_ = len /
sizeof(
PointT);
138 Eigen::Vector4f origin;
139 Eigen::Quaternionf orientation;
142 unsigned int data_index;
146 reader.
readHeader (*disk_storage_filename_, cloud_info, origin, orientation, pcd_version, data_type, data_index, 0);
153 disk_storage_filename_ = boost::shared_ptr<std::string> (
new std::string (path.string ()));
159 template<
typename Po
intT>
162 flushWritebuff (
true);
166 template<
typename Po
intT>
void
169 if (writebuff_.size () > 0)
174 cloud->width =
static_cast<uint32_t
> (writebuff_.size ());
177 cloud->points = writebuff_;
183 PCL_WARN (
"[pcl::outofcore::OutofcoreOctreeDiskContainer::%s] Flushing writebuffer in a dangerous way to file %s. This might overwrite data in destination file\n", __FUNCTION__, disk_storage_filename_->c_str ());
189 if (force_cache_dealloc)
191 writebuff_.resize (0);
197 template<
typename Po
intT>
PointT
200 PCL_THROW_EXCEPTION (
PCLException,
"[pcl::outofcore::OutofcoreOctreeDiskContainer] Not implemented for use with PCL library\n");
208 FILE* f = fopen (disk_storage_filename_->c_str (),
"rb");
212 int seekret = _fseeki64 (f, idx *
sizeof(
PointT), SEEK_SET);
214 assert (seekret == 0);
216 size_t readlen = fread (&temp, 1,
sizeof(
PointT), f);
218 assert (readlen ==
sizeof (
PointT));
220 int res = fclose (f);
227 if (idx < (filelen_ + writebuff_.size ()))
230 return (writebuff_[idx]);
234 PCL_THROW_EXCEPTION (
PCLException,
"[pcl::outofcore:OutofcoreOctreeDiskContainer] Index is out of range");
238 template<
typename Po
intT>
void
246 if ((start + count) > size ())
248 PCL_ERROR (
"[pcl::outofcore::OutofcoreOctreeDiskContainer::%s] Indicies out of range; start + count exceeds the size of the stored points\n", __FUNCTION__);
249 PCL_THROW_EXCEPTION (
PCLException,
"[pcl::outofcore::OutofcoreOctreeDiskContainer] Outofcore Octree Exception: Read indices exceed range");
255 int res = reader.
read (*disk_storage_filename_, *cloud);
259 for (
size_t i=0; i < cloud->
points.size (); i++)
260 dst.push_back (cloud->
points[i]);
265 template<
typename Po
intT>
void
275 uint64_t filestart = 0;
276 uint64_t filecount = 0;
278 int64_t buffstart = -1;
279 int64_t buffcount = -1;
281 if (start < filelen_)
286 if ((start + count) <= filelen_)
292 filecount = filelen_ - start;
295 buffcount = count - filecount;
301 boost::mutex::scoped_lock lock (rng_mutex_);
302 boost::bernoulli_distribution<double> buffdist (percent);
303 boost::variate_generator<boost::mt19937&, boost::bernoulli_distribution<double> > buffcoin (
rand_gen_, buffdist);
305 for (
size_t i = buffstart; i < static_cast<uint64_t> (buffcount); i++)
309 dst.push_back (writebuff_[i]);
318 std::vector < uint64_t > offsets;
320 boost::mutex::scoped_lock lock (rng_mutex_);
322 boost::bernoulli_distribution<double> filedist (percent);
323 boost::variate_generator<boost::mt19937&, boost::bernoulli_distribution<double> > filecoin (
rand_gen_, filedist);
324 for (uint64_t i = filestart; i < (filestart + filecount); i++)
328 offsets.push_back (i);
332 std::sort (offsets.begin (), offsets.end ());
334 FILE* f = fopen (disk_storage_filename_->c_str (),
"rb");
337 char* loc =
reinterpret_cast<char*
> (&p);
339 uint64_t filesamp = offsets.size ();
340 for (uint64_t i = 0; i < filesamp; i++)
342 int seekret = _fseeki64 (f, offsets[i] * static_cast<uint64_t> (
sizeof(
PointT)), SEEK_SET);
344 assert (seekret == 0);
345 size_t readlen = fread (loc,
sizeof(
PointT), 1, f);
347 assert (readlen == 1);
358 template<
typename Po
intT>
void
368 uint64_t filestart = 0;
369 uint64_t filecount = 0;
371 int64_t buffcount = -1;
373 if (start < filelen_)
378 if ((start + count) <= filelen_)
384 filecount = filelen_ - start;
385 buffcount = count - filecount;
388 uint64_t filesamp =
static_cast<uint64_t
> (percent *
static_cast<double> (filecount));
390 uint64_t buffsamp = (buffcount > 0) ? (static_cast<uint64_t > (percent * static_cast<double> (buffcount))) : 0;
392 if ((filesamp == 0) && (buffsamp == 0) && (size () > 0))
395 readRangeSubSample_bernoulli (start, count, percent, dst);
402 boost::mutex::scoped_lock lock (rng_mutex_);
404 boost::uniform_int < uint64_t > buffdist (0, buffcount - 1);
405 boost::variate_generator<boost::mt19937&, boost::uniform_int<uint64_t> > buffdie (
rand_gen_, buffdist);
407 for (uint64_t i = 0; i < buffsamp; i++)
409 uint64_t buffstart = buffdie ();
410 dst.push_back (writebuff_[buffstart]);
418 std::vector < uint64_t > offsets;
420 boost::mutex::scoped_lock lock (rng_mutex_);
422 offsets.resize (filesamp);
423 boost::uniform_int < uint64_t > filedist (filestart, filestart + filecount - 1);
424 boost::variate_generator<boost::mt19937&, boost::uniform_int<uint64_t> > filedie (
rand_gen_, filedist);
425 for (uint64_t i = 0; i < filesamp; i++)
427 uint64_t _filestart = filedie ();
428 offsets[i] = _filestart;
431 std::sort (offsets.begin (), offsets.end ());
433 FILE* f = fopen (disk_storage_filename_->c_str (),
"rb");
436 char* loc =
reinterpret_cast<char*
> (&p);
437 for (uint64_t i = 0; i < filesamp; i++)
439 int seekret = _fseeki64 (f, offsets[i] * static_cast<uint64_t> (
sizeof(
PointT)), SEEK_SET);
441 assert (seekret == 0);
442 size_t readlen = fread (loc,
sizeof(
PointT), 1, f);
444 assert (readlen == 1);
448 int res = fclose (f);
455 template<
typename Po
intT>
void
458 writebuff_.push_back (p);
459 if (writebuff_.size () > WRITE_BUFF_MAX_)
461 flushWritebuff (
false);
466 template<
typename Po
intT>
void
469 const uint64_t count = src.size ();
474 if (boost::filesystem::exists (*disk_storage_filename_))
478 int res = reader.
read (*disk_storage_filename_, *tmp_cloud);
485 tmp_cloud->
width =
static_cast<uint32_t
> (count + writebuff_.size ());
489 for (
size_t i = 0; i < src.size (); i++)
490 tmp_cloud->
points.push_back (src[i]);
493 for (
size_t i = 0; i < writebuff_.size (); i++)
495 tmp_cloud->
points.push_back (writebuff_[i]);
499 tmp_cloud->
width =
static_cast<uint32_t
> (tmp_cloud->
points.size ());
511 template<
typename Po
intT>
void
517 if (boost::filesystem::exists (*disk_storage_filename_))
521 int res = reader.
read (*disk_storage_filename_, *tmp_cloud);
525 PCL_DEBUG (
"[pcl::outofcore::OutofcoreOctreeDiskContainer::%s] Concatenating point cloud from %s to new cloud\n", __FUNCTION__, disk_storage_filename_->c_str ());
527 size_t previous_num_pts = tmp_cloud->width*tmp_cloud->height + input_cloud->width*input_cloud->height;
530 size_t res_pts = tmp_cloud->width*tmp_cloud->height;
532 (void)previous_num_pts;
535 assert (previous_num_pts == res_pts);
552 template<
typename Po
intT>
void
557 Eigen::Vector4f origin;
558 Eigen::Quaternionf orientation;
561 if (boost::filesystem::exists (*disk_storage_filename_))
564 int res = reader.
read (*disk_storage_filename_, *dst, origin, orientation, pcd_version);
570 PCL_ERROR (
"[pcl::outofcore::OutofcoreOctreeDiskContainer::%s] File %s does not exist in node.\n", __FUNCTION__, disk_storage_filename_->c_str ());
576 template<
typename Po
intT>
int
581 if (boost::filesystem::exists (*disk_storage_filename_))
592 PCL_ERROR (
"[pcl::outofcore::OutofcoreOctreeDiskContainer::%s] File %s does not exist in node.\n", __FUNCTION__, disk_storage_filename_->c_str ());
596 if(output_cloud.get () != 0)
602 output_cloud = temp_output_cloud;
609 template<
typename Po
intT>
void
617 for (
size_t i = 0; i < count; i++)
619 arr[i] = *(start[i]);
622 insertRange (arr, count);
628 template<
typename Po
intT>
void
634 if (boost::filesystem::exists (*disk_storage_filename_))
638 int res = reader.
read (disk_storage_filename_->c_str (), *tmp_cloud);
644 tmp_cloud->
width =
static_cast<uint32_t
> (count) + static_cast<uint32_t> (writebuff_.size ());
649 for (
size_t i = 0; i < writebuff_.size (); i++)
651 tmp_cloud->
points.push_back (writebuff_ [i]);
655 for (
size_t i = 0; i < count; i++)
657 tmp_cloud->
points.push_back (*(start + i));
660 tmp_cloud->
width =
static_cast<uint32_t
> (tmp_cloud->
points.size ());
672 template<
typename Po
intT> boost::uint64_t
676 Eigen::Vector4f origin;
677 Eigen::Quaternionf orientation;
680 unsigned int data_index;
684 reader.
readHeader (*disk_storage_filename_, cloud_info, origin, orientation, pcd_version, data_type, data_index, 0);
686 boost::uint64_t total_points = cloud_info.
width * cloud_info.
height + writebuff_.size ();
688 return (total_points);
695 #endif //PCL_OUTOFCORE_OCTREE_DISK_CONTAINER_IMPL_H_