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/random/variate_generator.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");
209 assert (f !=
nullptr);
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");
334 assert (f !=
nullptr);
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 auto 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");
433 assert (f !=
nullptr);
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);
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.
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.