40 #ifndef PCL_OUTOFCORE_OCTREE_DISK_CONTAINER_IMPL_H_
41 #define PCL_OUTOFCORE_OCTREE_DISK_CONTAINER_IMPL_H_
49 #include <boost/random/bernoulli_distribution.hpp>
50 #include <boost/random/uniform_int.hpp>
51 #include <boost/uuid/uuid_io.hpp>
54 #include <pcl/common/utils.h>
55 #include <pcl/io/pcd_io.h>
57 #include <pcl/PCLPointCloud2.h>
60 #include <pcl/outofcore/octree_disk_container.h>
64 #define _fseeki64 fseeko
65 #elif defined __MINGW32__
66 #define _fseeki64 fseeko64
73 template<
typename Po
intT>
74 std::mutex OutofcoreOctreeDiskContainer<PointT>::rng_mutex_;
76 template<
typename Po
intT> boost::mt19937
77 OutofcoreOctreeDiskContainer<PointT>::rand_gen_ (
static_cast<unsigned int> (std::time(
nullptr)));
79 template<
typename Po
intT>
82 template<
typename Po
intT>
84 template<
typename Po
intT>
87 template<
typename Po
intT>
void
92 std::lock_guard<std::mutex> lock (rng_mutex_);
102 template<
typename Po
intT>
112 template<
typename Po
intT>
117 if (boost::filesystem::exists (
path))
119 if (boost::filesystem::is_directory (
path))
123 boost::filesystem::path filename (uuid);
124 boost::filesystem::path file =
path / filename;
126 disk_storage_filename_ = file.string ();
130 std::uint64_t len = boost::filesystem::file_size (
path);
132 disk_storage_filename_ =
path.string ();
134 filelen_ = len /
sizeof(
PointT);
137 Eigen::Vector4f origin;
138 Eigen::Quaternionf orientation;
141 unsigned int data_index;
145 reader.
readHeader (disk_storage_filename_, cloud_info, origin, orientation, pcd_version, data_type, data_index, 0);
152 disk_storage_filename_ =
path.string ();
158 template<
typename Po
intT>
161 flushWritebuff (
true);
165 template<
typename Po
intT>
void
168 if (!writebuff_.empty ())
173 cloud->width = writebuff_.size ();
176 cloud->points = writebuff_;
182 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 ());
188 if (force_cache_dealloc)
190 writebuff_.resize (0);
196 template<
typename Po
intT>
PointT
199 PCL_THROW_EXCEPTION (
PCLException,
"[pcl::outofcore::OutofcoreOctreeDiskContainer] Not implemented for use with PCL library\n");
207 FILE* f = fopen (disk_storage_filename_.c_str (),
"rbe");
208 assert (f !=
nullptr);
211 int seekret = _fseeki64 (f, idx *
sizeof(
PointT), SEEK_SET);
213 assert (seekret == 0);
215 std::size_t readlen = fread (&temp, 1,
sizeof(
PointT), f);
217 assert (readlen ==
sizeof (
PointT));
219 int res = fclose (f);
226 if (idx < (filelen_ + writebuff_.size ()))
229 return (writebuff_[idx]);
233 PCL_THROW_EXCEPTION (
PCLException,
"[pcl::outofcore:OutofcoreOctreeDiskContainer] Index is out of range");
237 template<
typename Po
intT>
void
245 if ((start + count) > size ())
247 PCL_ERROR (
"[pcl::outofcore::OutofcoreOctreeDiskContainer::%s] Indices out of range; start + count exceeds the size of the stored points\n", __FUNCTION__);
248 PCL_THROW_EXCEPTION (
PCLException,
"[pcl::outofcore::OutofcoreOctreeDiskContainer] Outofcore Octree Exception: Read indices exceed range");
254 int res = reader.
read (disk_storage_filename_, *cloud);
258 dst.insert(dst.end(), cloud->
cbegin(), cloud->
cend());
263 template<
typename Po
intT>
void
273 std::uint64_t filestart = 0;
274 std::uint64_t filecount = 0;
276 std::int64_t buffstart = -1;
277 std::int64_t buffcount = -1;
279 if (start < filelen_)
284 if ((start + count) <= filelen_)
290 filecount = filelen_ - start;
293 buffcount = count - filecount;
299 std::lock_guard<std::mutex> lock (rng_mutex_);
300 boost::bernoulli_distribution<double> buffdist (percent);
301 boost::variate_generator<boost::mt19937&, boost::bernoulli_distribution<double> > buffcoin (
rand_gen_, buffdist);
303 for (std::size_t i = buffstart; i < static_cast<std::uint64_t> (buffcount); i++)
307 dst.push_back (writebuff_[i]);
316 std::vector < std::uint64_t > offsets;
318 std::lock_guard<std::mutex> lock (rng_mutex_);
320 boost::bernoulli_distribution<double> filedist (percent);
321 boost::variate_generator<boost::mt19937&, boost::bernoulli_distribution<double> > filecoin (
rand_gen_, filedist);
322 for (std::uint64_t i = filestart; i < (filestart + filecount); i++)
326 offsets.push_back (i);
330 std::sort (offsets.begin (), offsets.end ());
332 FILE* f = fopen (disk_storage_filename_.c_str (),
"rbe");
333 assert (f !=
nullptr);
335 char* loc =
reinterpret_cast<char*
> (&p);
337 std::uint64_t filesamp = offsets.size ();
338 for (std::uint64_t i = 0; i < filesamp; i++)
340 int seekret = _fseeki64 (f, offsets[i] *
static_cast<std::uint64_t
> (
sizeof(
PointT)), SEEK_SET);
342 assert (seekret == 0);
343 std::size_t readlen = fread (loc,
sizeof(
PointT), 1, f);
345 assert (readlen == 1);
356 template<
typename Po
intT>
void
366 std::uint64_t filestart = 0;
367 std::uint64_t filecount = 0;
369 std::int64_t buffcount = -1;
371 if (start < filelen_)
376 if ((start + count) <= filelen_)
382 filecount = filelen_ - start;
383 buffcount = count - filecount;
386 auto filesamp =
static_cast<std::uint64_t
> (percent *
static_cast<double> (filecount));
388 std::uint64_t buffsamp = (buffcount > 0) ? (
static_cast<std::uint64_t
> (percent *
static_cast<double> (buffcount))) : 0;
390 if ((filesamp == 0) && (buffsamp == 0) && (size () > 0))
393 readRangeSubSample_bernoulli (start, count, percent, dst);
400 std::lock_guard<std::mutex> lock (rng_mutex_);
402 boost::uniform_int < std::uint64_t > buffdist (0, buffcount - 1);
403 boost::variate_generator<boost::mt19937&, boost::uniform_int<std::uint64_t> > buffdie (
rand_gen_, buffdist);
405 for (std::uint64_t i = 0; i < buffsamp; i++)
407 std::uint64_t buffstart = buffdie ();
408 dst.push_back (writebuff_[buffstart]);
416 std::vector < std::uint64_t > offsets;
418 std::lock_guard<std::mutex> lock (rng_mutex_);
420 offsets.resize (filesamp);
421 boost::uniform_int < std::uint64_t > filedist (filestart, filestart + filecount - 1);
422 boost::variate_generator<boost::mt19937&, boost::uniform_int<std::uint64_t> > filedie (
rand_gen_, filedist);
423 for (std::uint64_t i = 0; i < filesamp; i++)
425 std::uint64_t _filestart = filedie ();
426 offsets[i] = _filestart;
429 std::sort (offsets.begin (), offsets.end ());
431 FILE* f = fopen (disk_storage_filename_.c_str (),
"rbe");
432 assert (f !=
nullptr);
434 char* loc =
reinterpret_cast<char*
> (&p);
435 for (std::uint64_t i = 0; i < filesamp; i++)
437 int seekret = _fseeki64 (f, offsets[i] *
static_cast<std::uint64_t
> (
sizeof(
PointT)), SEEK_SET);
439 assert (seekret == 0);
440 std::size_t readlen = fread (loc,
sizeof(
PointT), 1, f);
442 assert (readlen == 1);
446 int res = fclose (f);
453 template<
typename Po
intT>
void
456 writebuff_.push_back (p);
457 if (writebuff_.size () > WRITE_BUFF_MAX_)
459 flushWritebuff (
false);
464 template<
typename Po
intT>
void
467 const std::uint64_t count = src.size ();
472 if (boost::filesystem::exists (disk_storage_filename_))
476 int res = reader.
read (disk_storage_filename_, *tmp_cloud);
483 tmp_cloud->
width = count + writebuff_.size ();
487 for (std::size_t i = 0; i < src.size (); i++)
491 for (std::size_t i = 0; i < writebuff_.size (); i++)
509 template<
typename Po
intT>
void
515 if (boost::filesystem::exists (disk_storage_filename_))
519 int res = reader.
read (disk_storage_filename_, *tmp_cloud);
523 PCL_DEBUG (
"[pcl::outofcore::OutofcoreOctreeDiskContainer::%s] Concatenating point cloud from %s to new cloud\n", __FUNCTION__, disk_storage_filename_.c_str ());
525 std::size_t previous_num_pts = tmp_cloud->width*tmp_cloud->height + input_cloud->width*input_cloud->height;
528 std::size_t res_pts = tmp_cloud->width*tmp_cloud->height;
533 assert (previous_num_pts == res_pts);
550 template<
typename Po
intT>
void
555 Eigen::Vector4f origin;
556 Eigen::Quaternionf orientation;
558 if (boost::filesystem::exists (disk_storage_filename_))
562 int res = reader.
read (disk_storage_filename_, *dst, origin, orientation, pcd_version);
568 PCL_ERROR (
"[pcl::outofcore::OutofcoreOctreeDiskContainer::%s] File %s does not exist in node.\n", __FUNCTION__, disk_storage_filename_.c_str ());
574 template<
typename Po
intT>
int
579 if (boost::filesystem::exists (disk_storage_filename_))
590 PCL_ERROR (
"[pcl::outofcore::OutofcoreOctreeDiskContainer::%s] File %s does not exist in node.\n", __FUNCTION__, disk_storage_filename_.c_str ());
594 if(output_cloud.get () !=
nullptr)
600 output_cloud = temp_output_cloud;
607 template<
typename Po
intT>
void
615 for (std::size_t i = 0; i < count; i++)
617 arr[i] = *(start[i]);
620 insertRange (arr, count);
626 template<
typename Po
intT>
void
632 if (boost::filesystem::exists (disk_storage_filename_))
636 int res = reader.
read (disk_storage_filename_, *tmp_cloud);
642 tmp_cloud->
width = count + writebuff_.size ();
647 for (std::size_t i = 0; i < writebuff_.size (); i++)
653 for (std::size_t i = 0; i < count; i++)
670 template<
typename Po
intT> std::uint64_t
674 Eigen::Vector4f origin;
675 Eigen::Quaternionf orientation;
678 unsigned int data_index;
682 reader.
readHeader (disk_storage_filename_, cloud_info, origin, orientation, pcd_version, data_type, data_index, 0);
684 std::uint64_t total_points = cloud_info.
width * cloud_info.
height + writebuff_.size ();
686 return (total_points);
Point Cloud Data (PCD) file format reader.
int readHeader(std::istream &binary_istream, pcl::PCLPointCloud2 &cloud, Eigen::Vector4f &origin, Eigen::Quaternionf &orientation, int &pcd_version, int &data_type, unsigned int &data_idx)
Read a point cloud data header from a PCD-formatted, binary istream.
int read(const std::string &file_name, pcl::PCLPointCloud2 &cloud, Eigen::Vector4f &origin, Eigen::Quaternionf &orientation, int &pcd_version, const int offset=0) override
Read a point cloud data from a PCD file and store it into a pcl/PCLPointCloud2.
Point Cloud Data (PCD) file format writer.
int writeBinaryCompressed(const std::string &file_name, const pcl::PCLPointCloud2 &cloud, const Eigen::Vector4f &origin=Eigen::Vector4f::Zero(), const Eigen::Quaternionf &orientation=Eigen::Quaternionf::Identity())
Save point cloud data to a PCD file containing n-D points, in BINARY_COMPRESSED format.
A base class for all pcl exceptions which inherits from std::runtime_error.
PointCloud represents the base class in PCL for storing collections of 3D points.
const_iterator cbegin() const noexcept
void push_back(const PointT &pt)
Insert a new point in the cloud, at the end of the container.
const_iterator cend() const noexcept
std::uint32_t width
The point cloud width (if organized as an image-structure).
std::uint32_t height
The point cloud height (if organized as an image-structure).
shared_ptr< PointCloud< PointT > > Ptr
Class responsible for serialization and deserialization of out of core point data.
int read(pcl::PCLPointCloud2::Ptr &output_cloud)
Reads the entire point contents from disk into output_cloud.
OutofcoreOctreeDiskContainer()
Empty constructor creates disk container and sets filename from random uuid string.
static void getRandomUUIDString(std::string &s)
Generate a universally unique identifier (UUID)
void insertRange(const AlignedPointTVector &src)
Inserts a vector of points into the disk data structure.
void readRangeSubSample_bernoulli(const std::uint64_t start, const std::uint64_t count, const double percent, AlignedPointTVector &dst)
Use bernoulli trials to select points.
void readRange(const std::uint64_t start, const std::uint64_t count, AlignedPointTVector &dst) override
Reads count points into memory from the disk container.
std::uint64_t getDataSize() const
Returns the number of points in the PCD file by reading the PCD header.
std::string & path()
Returns this objects path name.
typename OutofcoreAbstractNodeContainer< PointT >::AlignedPointTVector AlignedPointTVector
void readRangeSubSample(const std::uint64_t start, const std::uint64_t count, const double percent, AlignedPointTVector &dst) override
grab percent*count random points.
void push_back(const PointT &p)
Adds a single point to the buffer to be written to disk when the buffer grows sufficiently large,...
PointT operator[](std::uint64_t idx) const override
provides random access to points based on a linear index
~OutofcoreOctreeDiskContainer() override
flushes write buffer, then frees memory
Defines all the PCL implemented PointT point type structures.
PCL_EXPORTS bool concatenate(const pcl::PointCloud< PointT > &cloud1, const pcl::PointCloud< PointT > &cloud2, pcl::PointCloud< PointT > &cloud_out)
Concatenate two pcl::PointCloud<PointT>
int loadPCDFile(const std::string &file_name, pcl::PCLPointCloud2 &cloud)
Load a PCD v.6 file into a templated PointCloud type.
boost::uuids::basic_random_generator< boost::mt19937 > OutofcoreOctreeDiskContainer< PointT >::uuid_gen_ & rand_gen_
void ignore(const T &...)
Utility function to eliminate unused variable warnings.
shared_ptr< ::pcl::PCLPointCloud2 > Ptr
A point structure representing Euclidean xyz coordinates, and the RGB color.