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_
boost::uint64_t getDataSize() const
Returns the number of points in the PCD file by reading the PCD header.
boost::uuids::random_generator OutofcoreOctreeDiskContainer< PointT >::uuid_gen_ & rand_gen_
boost::shared_ptr< ::pcl::PCLPointCloud2 > Ptr
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.
Point Cloud Data (PCD) file format reader.
~OutofcoreOctreeDiskContainer()
flushes write buffer, then frees memory
void readRange(const uint64_t start, const uint64_t count, AlignedPointTVector &dst)
Reads count points into memory from the disk container.
int loadPCDFile(const std::string &file_name, pcl::PCLPointCloud2 &cloud)
Load a PCD v.6 file into a templated PointCloud type.
OutofcoreOctreeDiskContainer()
Empty constructor creates disk container and sets filename from random uuid string.
uint32_t width
The point cloud width (if organized as an image-structure).
A base class for all pcl exceptions which inherits from std::runtime_error.
int readHeader(const std::string &file_name, pcl::PCLPointCloud2 &cloud, Eigen::Vector4f &origin, Eigen::Quaternionf &orientation, int &pcd_version, int &data_type, unsigned int &data_idx, const int offset=0)
Read a point cloud data header from a PCD file.
void readRangeSubSample(const uint64_t start, const uint64_t count, const double percent, AlignedPointTVector &dst)
grab percent*count random points.
PointT operator[](uint64_t idx) const
provides random access to points based on a linear index
void insertRange(const AlignedPointTVector &src)
Inserts a vector of points into the disk data structure.
void push_back(const PointT &p)
Adds a single point to the buffer to be written to disk when the buffer grows sufficiently large...
boost::shared_ptr< PointCloud< PointT > > Ptr
uint32_t height
The point cloud height (if organized as an image-structure).
PCL_EXPORTS bool concatenatePointCloud(const pcl::PCLPointCloud2 &cloud1, const pcl::PCLPointCloud2 &cloud2, pcl::PCLPointCloud2 &cloud_out)
Concatenate two pcl::PCLPointCloud2.
static void getRandomUUIDString(std::string &s)
Generate a universally unique identifier (UUID)
int read(pcl::PCLPointCloud2::Ptr &output_cloud)
Reads the entire point contents from disk into output_cloud.
std::vector< PointT, Eigen::aligned_allocator< PointT > > points
The point data.
OutofcoreAbstractNodeContainer< PointT >::AlignedPointTVector AlignedPointTVector
Point Cloud Data (PCD) file format writer.
A point structure representing Euclidean xyz coordinates, and the RGB color.
void readRangeSubSample_bernoulli(const uint64_t start, const uint64_t count, const double percent, AlignedPointTVector &dst)
Use bernoulli trials to select points.
int read(const std::string &file_name, pcl::PCLPointCloud2 &cloud, Eigen::Vector4f &origin, Eigen::Quaternionf &orientation, int &pcd_version, const int offset=0)
Read a point cloud data from a PCD file and store it into a pcl/PCLPointCloud2.
Class responsible for serialization and deserialization of out of core point data.