40 #ifndef PCL_OUTOFCORE_OCTREE_DISK_CONTAINER_IMPL_H_
41 #define PCL_OUTOFCORE_OCTREE_DISK_CONTAINER_IMPL_H_
49 #include <pcl/outofcore/boost.h>
50 #include <boost/random/bernoulli_distribution.hpp>
51 #include <boost/random/uniform_int.hpp>
52 #include <boost/uuid/uuid_io.hpp>
55 #include <pcl/common/utils.h>
56 #include <pcl/io/pcd_io.h>
58 #include <pcl/PCLPointCloud2.h>
61 #include <pcl/outofcore/octree_disk_container.h>
65 #define _fseeki64 fseeko
66 #elif defined __MINGW32__
67 #define _fseeki64 fseeko64
74 template<
typename Po
intT>
75 std::mutex OutofcoreOctreeDiskContainer<PointT>::rng_mutex_;
77 template<
typename Po
intT> boost::mt19937
78 OutofcoreOctreeDiskContainer<PointT>::rand_gen_ (
static_cast<unsigned int> (std::time(
nullptr)));
80 template<
typename Po
intT>
83 template<
typename Po
intT>
85 template<
typename Po
intT>
88 template<
typename Po
intT>
void
93 std::lock_guard<std::mutex> lock (rng_mutex_);
103 template<
typename Po
intT>
113 template<
typename Po
intT>
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_ = file.string ();
131 std::uint64_t len = boost::filesystem::file_size (
path);
133 disk_storage_filename_ =
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_ =
path.string ();
159 template<
typename Po
intT>
162 flushWritebuff (
true);
166 template<
typename Po
intT>
void
169 if (!writebuff_.empty ())
174 cloud->width = 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 (),
"rbe");
212 int seekret = _fseeki64 (f, idx *
sizeof(
PointT), SEEK_SET);
214 assert (seekret == 0);
216 std::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] Indices 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 dst.insert(dst.end(), cloud->
cbegin(), cloud->
cend());
264 template<
typename Po
intT>
void
274 std::uint64_t filestart = 0;
275 std::uint64_t filecount = 0;
277 std::int64_t buffstart = -1;
278 std::int64_t buffcount = -1;
280 if (start < filelen_)
285 if ((start + count) <= filelen_)
291 filecount = filelen_ - start;
294 buffcount = count - filecount;
300 std::lock_guard<std::mutex> lock (rng_mutex_);
301 boost::bernoulli_distribution<double> buffdist (percent);
302 boost::variate_generator<boost::mt19937&, boost::bernoulli_distribution<double> > buffcoin (
rand_gen_, buffdist);
304 for (std::size_t i = buffstart; i < static_cast<std::uint64_t> (buffcount); i++)
308 dst.push_back (writebuff_[i]);
317 std::vector < std::uint64_t > offsets;
319 std::lock_guard<std::mutex> lock (rng_mutex_);
321 boost::bernoulli_distribution<double> filedist (percent);
322 boost::variate_generator<boost::mt19937&, boost::bernoulli_distribution<double> > filecoin (
rand_gen_, filedist);
323 for (std::uint64_t i = filestart; i < (filestart + filecount); i++)
327 offsets.push_back (i);
331 std::sort (offsets.begin (), offsets.end ());
333 FILE* f = fopen (disk_storage_filename_.c_str (),
"rbe");
336 char* loc =
reinterpret_cast<char*
> (&p);
338 std::uint64_t filesamp = offsets.size ();
339 for (std::uint64_t i = 0; i < filesamp; i++)
341 int seekret = _fseeki64 (f, offsets[i] *
static_cast<std::uint64_t
> (
sizeof(
PointT)), SEEK_SET);
343 assert (seekret == 0);
344 std::size_t readlen = fread (loc,
sizeof(
PointT), 1, f);
346 assert (readlen == 1);
357 template<
typename Po
intT>
void
367 std::uint64_t filestart = 0;
368 std::uint64_t filecount = 0;
370 std::int64_t buffcount = -1;
372 if (start < filelen_)
377 if ((start + count) <= filelen_)
383 filecount = filelen_ - start;
384 buffcount = count - filecount;
387 std::uint64_t filesamp =
static_cast<std::uint64_t
> (percent *
static_cast<double> (filecount));
389 std::uint64_t buffsamp = (buffcount > 0) ? (
static_cast<std::uint64_t
> (percent *
static_cast<double> (buffcount))) : 0;
391 if ((filesamp == 0) && (buffsamp == 0) && (size () > 0))
394 readRangeSubSample_bernoulli (start, count, percent, dst);
401 std::lock_guard<std::mutex> lock (rng_mutex_);
403 boost::uniform_int < std::uint64_t > buffdist (0, buffcount - 1);
404 boost::variate_generator<boost::mt19937&, boost::uniform_int<std::uint64_t> > buffdie (
rand_gen_, buffdist);
406 for (std::uint64_t i = 0; i < buffsamp; i++)
408 std::uint64_t buffstart = buffdie ();
409 dst.push_back (writebuff_[buffstart]);
417 std::vector < std::uint64_t > offsets;
419 std::lock_guard<std::mutex> lock (rng_mutex_);
421 offsets.resize (filesamp);
422 boost::uniform_int < std::uint64_t > filedist (filestart, filestart + filecount - 1);
423 boost::variate_generator<boost::mt19937&, boost::uniform_int<std::uint64_t> > filedie (
rand_gen_, filedist);
424 for (std::uint64_t i = 0; i < filesamp; i++)
426 std::uint64_t _filestart = filedie ();
427 offsets[i] = _filestart;
430 std::sort (offsets.begin (), offsets.end ());
432 FILE* f = fopen (disk_storage_filename_.c_str (),
"rbe");
435 char* loc =
reinterpret_cast<char*
> (&p);
436 for (std::uint64_t i = 0; i < filesamp; i++)
438 int seekret = _fseeki64 (f, offsets[i] *
static_cast<std::uint64_t
> (
sizeof(
PointT)), SEEK_SET);
440 assert (seekret == 0);
441 std::size_t readlen = fread (loc,
sizeof(
PointT), 1, f);
443 assert (readlen == 1);
447 int res = fclose (f);
454 template<
typename Po
intT>
void
457 writebuff_.push_back (p);
458 if (writebuff_.size () > WRITE_BUFF_MAX_)
460 flushWritebuff (
false);
465 template<
typename Po
intT>
void
468 const std::uint64_t count = src.size ();
473 if (boost::filesystem::exists (disk_storage_filename_))
477 int res = reader.
read (disk_storage_filename_, *tmp_cloud);
484 tmp_cloud->
width = count + writebuff_.size ();
488 for (std::size_t i = 0; i < src.size (); i++)
492 for (std::size_t i = 0; i < writebuff_.size (); i++)
510 template<
typename Po
intT>
void
516 if (boost::filesystem::exists (disk_storage_filename_))
520 int res = reader.
read (disk_storage_filename_, *tmp_cloud);
524 PCL_DEBUG (
"[pcl::outofcore::OutofcoreOctreeDiskContainer::%s] Concatenating point cloud from %s to new cloud\n", __FUNCTION__, disk_storage_filename_.c_str ());
526 std::size_t previous_num_pts = tmp_cloud->width*tmp_cloud->height + input_cloud->width*input_cloud->height;
529 std::size_t res_pts = tmp_cloud->width*tmp_cloud->height;
534 assert (previous_num_pts == res_pts);
551 template<
typename Po
intT>
void
556 Eigen::Vector4f origin;
557 Eigen::Quaternionf orientation;
559 if (boost::filesystem::exists (disk_storage_filename_))
563 int res = reader.
read (disk_storage_filename_, *dst, origin, orientation, pcd_version);
569 PCL_ERROR (
"[pcl::outofcore::OutofcoreOctreeDiskContainer::%s] File %s does not exist in node.\n", __FUNCTION__, disk_storage_filename_.c_str ());
575 template<
typename Po
intT>
int
580 if (boost::filesystem::exists (disk_storage_filename_))
591 PCL_ERROR (
"[pcl::outofcore::OutofcoreOctreeDiskContainer::%s] File %s does not exist in node.\n", __FUNCTION__, disk_storage_filename_.c_str ());
595 if(output_cloud.get () !=
nullptr)
601 output_cloud = temp_output_cloud;
608 template<
typename Po
intT>
void
616 for (std::size_t i = 0; i < count; i++)
618 arr[i] = *(start[i]);
621 insertRange (arr, count);
627 template<
typename Po
intT>
void
633 if (boost::filesystem::exists (disk_storage_filename_))
637 int res = reader.
read (disk_storage_filename_, *tmp_cloud);
643 tmp_cloud->
width = count + writebuff_.size ();
648 for (std::size_t i = 0; i < writebuff_.size (); i++)
654 for (std::size_t i = 0; i < count; i++)
671 template<
typename Po
intT> std::uint64_t
675 Eigen::Vector4f origin;
676 Eigen::Quaternionf orientation;
679 unsigned int data_index;
683 reader.
readHeader (disk_storage_filename_, cloud_info, origin, orientation, pcd_version, data_type, data_index, 0);
685 std::uint64_t total_points = cloud_info.
width * cloud_info.
height + writebuff_.size ();
687 return (total_points);
694 #endif //PCL_OUTOFCORE_OCTREE_DISK_CONTAINER_IMPL_H_