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))
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]);
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_ void readRangeSubSample(const uint64_t start, const uint64_t count, const double percent, AlignedPointTVector &dst)
grab percent*count random points.
std::vector< PointT, Eigen::aligned_allocator< PointT > > points
The point data.
uint64_t size() const
Returns the total number of points for which this container is responsible, filelen_ + points in writ...
A base class for all pcl exceptions which inherits from std::runtime_error.
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.
void insertRange(const AlignedPointTVector &src)
Inserts a vector of points into the disk data structure.
static void getRandomUUIDString(std::string &s)
Generate a universally unique identifier (UUID)
std::string & path()
Returns this objects path name.
void readRange(const uint64_t start, const uint64_t count, AlignedPointTVector &dst)
Reads count points into memory from the disk container.
PointT operator[](uint64_t idx) const
provides random access to points based on a linear index
boost::shared_ptr< ::pcl::PCLPointCloud2 > Ptr
uint32_t height
The point cloud height (if organized as an image-structure).
Class responsible for serialization and deserialization of out of core point data.
boost::shared_ptr< PointCloud< PointT > > Ptr
uint32_t width
The point cloud width (if organized as an image-structure).
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.
~OutofcoreOctreeDiskContainer()
flushes write buffer, then frees memory
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::uint64_t getDataSize() const
Returns the number of points in the PCD file by reading the PCD header.
OutofcoreOctreeDiskContainer()
Empty constructor creates disk container and sets filename from random uuid string.
PointCloud represents the base class in PCL for storing collections of 3D points. ...
void readRangeSubSample_bernoulli(const uint64_t start, const uint64_t count, const double percent, AlignedPointTVector &dst)
Use bernoulli trials to select points.
int loadPCDFile(const std::string &file_name, pcl::PCLPointCloud2 &cloud)
Load a PCD v.6 file into a templated PointCloud type.
boost::uuids::random_generator OutofcoreOctreeDiskContainer< PointT >::uuid_gen_ & rand_gen_
int read(pcl::PCLPointCloud2::Ptr &output_cloud)
Reads the entire point contents from disk into output_cloud.
OutofcoreAbstractNodeContainer< PointT >::AlignedPointTVector AlignedPointTVector
PCL_EXPORTS bool concatenatePointCloud(const pcl::PCLPointCloud2 &cloud1, const pcl::PCLPointCloud2 &cloud2, pcl::PCLPointCloud2 &cloud_out)
Concatenate two pcl::PCLPointCloud2.
Point Cloud Data (PCD) file format reader.
A point structure representing Euclidean xyz coordinates, and the RGB color.
Point Cloud Data (PCD) file format writer.
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.