Point Cloud Library (PCL)  1.11.1
octree_disk_container.h
1 /*
2  * Software License Agreement (BSD License)
3  *
4  * Point Cloud Library (PCL) - www.pointclouds.org
5  * Copyright (c) 2010-2012, Willow Garage, Inc.
6  * Copyright (c) 2012, Urban Robotics, Inc.
7  *
8  * All rights reserved.
9  *
10  * Redistribution and use in source and binary forms, with or without
11  * modification, are permitted provided that the following conditions
12  * are met:
13  *
14  * * Redistributions of source code must retain the above copyright
15  * notice, this list of conditions and the following disclaimer.
16  * * Redistributions in binary form must reproduce the above
17  * copyright notice, this list of conditions and the following
18  * disclaimer in the documentation and/or other materials provided
19  * with the distribution.
20  * * Neither the name of Willow Garage, Inc. nor the names of its
21  * contributors may be used to endorse or promote products derived
22  * from this software without specific prior written permission.
23  *
24  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
25  * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
26  * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
27  * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
28  * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
29  * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
30  * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
31  * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
32  * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
33  * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
34  * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
35  * POSSIBILITY OF SUCH DAMAGE.
36  *
37  * $Id: octree_disk_container.h 6927M 2012-08-24 13:26:40Z (local) $
38  */
39 
40 #pragma once
41 
42 // C++
43 #include <mutex>
44 #include <vector>
45 #include <string>
46 
47 // Boost
48 #include <boost/uuid/random_generator.hpp>
49 
50 #include <pcl/common/utils.h> // pcl::utils::ignore
51 #include <pcl/outofcore/boost.h>
52 #include <pcl/outofcore/octree_abstract_node_container.h>
53 #include <pcl/io/pcd_io.h>
54 #include <pcl/PCLPointCloud2.h>
55 
56 //allows operation on POSIX
57 #if !defined _WIN32
58 #define _fseeki64 fseeko
59 #elif defined __MINGW32__
60 #define _fseeki64 fseeko64
61 #endif
62 
63 namespace pcl
64 {
65  namespace outofcore
66  {
67  /** \class OutofcoreOctreeDiskContainer
68  * \note Code was adapted from the Urban Robotics out of core octree implementation.
69  * Contact Jacob Schloss <jacob.schloss@urbanrobotics.net> with any questions.
70  * http://www.urbanrobotics.net/
71  *
72  * \brief Class responsible for serialization and deserialization of out of core point data
73  * \ingroup outofcore
74  * \author Jacob Schloss (jacob.schloss@urbanrobotics.net)
75  */
76  template<typename PointT = pcl::PointXYZ>
78  {
79 
80  public:
82 
83  /** \brief Empty constructor creates disk container and sets filename from random uuid string*/
85 
86  /** \brief Creates uuid named file or loads existing file
87  *
88  * If \b dir is a directory, this constructor will create a new
89  * uuid named file; if \b dir is an existing file, it will load the
90  * file metadata for accessing the tree.
91  *
92  * \param[in] dir Path to the tree. If it is a directory, it
93  * will create the metadata. If it is a file, it will load the metadata into memory.
94  */
95  OutofcoreOctreeDiskContainer (const boost::filesystem::path &dir);
96 
97  /** \brief flushes write buffer, then frees memory */
99 
100  /** \brief provides random access to points based on a linear index
101  */
102  inline PointT
103  operator[] (std::uint64_t idx) const override;
104 
105  /** \brief Adds a single point to the buffer to be written to disk when the buffer grows sufficiently large, the object is destroyed, or the write buffer is manually flushed */
106  inline void
107  push_back (const PointT& p);
108 
109  /** \brief Inserts a vector of points into the disk data structure */
110  void
111  insertRange (const AlignedPointTVector& src);
112 
113  /** \brief Inserts a PCLPointCloud2 object directly into the disk container */
114  void
115  insertRange (const pcl::PCLPointCloud2::Ptr &input_cloud);
116 
117  void
118  insertRange (const PointT* const * start, const std::uint64_t count) override;
119 
120  /** \brief This is the primary method for serialization of
121  * blocks of point data. This is called by the outofcore
122  * octree interface, opens the binary file for appending data,
123  * and writes it to disk.
124  *
125  * \param[in] start address of the first point to insert
126  * \param[in] count offset from start of the last point to insert
127  */
128  void
129  insertRange (const PointT* start, const std::uint64_t count) override;
130 
131  /** \brief Reads \b count points into memory from the disk container
132  *
133  * Reads \b count points into memory from the disk container, reading at most 2 million elements at a time
134  *
135  * \param[in] start index of first point to read from disk
136  * \param[in] count offset of last point to read from disk
137  * \param[out] dst std::vector as destination for points read from disk into memory
138  */
139  void
140  readRange (const std::uint64_t start, const std::uint64_t count, AlignedPointTVector &dst) override;
141 
142  void
144 
145  /** \brief Reads the entire point contents from disk into \c output_cloud
146  * \param[out] output_cloud
147  */
148  int
149  read (pcl::PCLPointCloud2::Ptr &output_cloud);
150 
151  /** \brief grab percent*count random points. points are \b not guaranteed to be
152  * unique (could have multiple identical points!)
153  *
154  * \param[in] start The starting index of points to select
155  * \param[in] count The length of the range of points from which to randomly sample
156  * (i.e. from start to start+count)
157  * \param[in] percent The percentage of count that is enough points to make up this random sample
158  * \param[out] dst std::vector as destination for randomly sampled points; size will
159  * be percentage*count
160  */
161  void
162  readRangeSubSample (const std::uint64_t start, const std::uint64_t count, const double percent,
163  AlignedPointTVector &dst) override;
164 
165  /** \brief Use bernoulli trials to select points. All points selected will be unique.
166  *
167  * \param[in] start The starting index of points to select
168  * \param[in] count The length of the range of points from which to randomly sample
169  * (i.e. from start to start+count)
170  * \param[in] percent The percentage of count that is enough points to make up this random sample
171  * \param[out] dst std::vector as destination for randomly sampled points; size will
172  * be percentage*count
173  */
174  void
175  readRangeSubSample_bernoulli (const std::uint64_t start, const std::uint64_t count,
176  const double percent, AlignedPointTVector& dst);
177 
178  /** \brief Returns the total number of points for which this container is responsible, \c filelen_ + points in \c writebuff_ that have not yet been flushed to the disk
179  */
181  size () const override
182  {
183  return (filelen_ + writebuff_.size ());
184  }
185 
186  /** \brief STL-like empty test
187  * \return true if container has no data on disk or waiting to be written in \c writebuff_ */
188  inline bool
189  empty () const override
190  {
191  return ((filelen_ == 0) && writebuff_.empty ());
192  }
193 
194  /** \brief Exposed functionality for manually flushing the write buffer during tree creation */
195  void
196  flush (const bool force_cache_dealloc)
197  {
198  flushWritebuff (force_cache_dealloc);
199  }
200 
201  /** \brief Returns this objects path name */
202  inline std::string&
203  path ()
204  {
205  return (disk_storage_filename_);
206  }
207 
208  inline void
209  clear () override
210  {
211  //clear elements that have not yet been written to disk
212  writebuff_.clear ();
213  //remove the binary data in the directory
214  PCL_DEBUG ("[Octree Disk Container] Removing the point data from disk, in file %s\n", disk_storage_filename_.c_str ());
215  boost::filesystem::remove (boost::filesystem::path (disk_storage_filename_.c_str ()));
216  //reset the size-of-file counter
217  filelen_ = 0;
218  }
219 
220  /** \brief write points to disk as ascii
221  *
222  * \param[in] path
223  */
224  void
225  convertToXYZ (const boost::filesystem::path &path) override
226  {
227  if (boost::filesystem::exists (disk_storage_filename_))
228  {
229  FILE* fxyz = fopen (path.string ().c_str (), "we");
230 
231  FILE* f = fopen (disk_storage_filename_.c_str (), "rbe");
232  assert (f != NULL);
233 
234  std::uint64_t num = size ();
235  PointT p;
236  char* loc = reinterpret_cast<char*> ( &p );
237 
238  for (std::uint64_t i = 0; i < num; i++)
239  {
240  int seekret = _fseeki64 (f, i * sizeof (PointT), SEEK_SET);
241  pcl::utils::ignore(seekret);
242  assert (seekret == 0);
243  std::size_t readlen = fread (loc, sizeof (PointT), 1, f);
244  pcl::utils::ignore(readlen);
245  assert (readlen == 1);
246 
247  //of << p.x << "\t" << p.y << "\t" << p.z << "\n";
248  std::stringstream ss;
249  ss << std::fixed;
250  ss.precision (16);
251  ss << p.x << "\t" << p.y << "\t" << p.z << "\n";
252 
253  fwrite (ss.str ().c_str (), 1, ss.str ().size (), fxyz);
254  }
255  int res = fclose (f);
256  pcl::utils::ignore(res);
257  assert (res == 0);
258  res = fclose (fxyz);
259  assert (res == 0);
260  }
261  }
262 
263  /** \brief Generate a universally unique identifier (UUID)
264  *
265  * A mutex lock happens to ensure uniqueness
266  *
267  */
268  static void
269  getRandomUUIDString (std::string &s);
270 
271  /** \brief Returns the number of points in the PCD file by reading the PCD header. */
273  getDataSize () const;
274 
275  private:
276  //no copy construction
278 
279 
281  operator= (const OutofcoreOctreeDiskContainer& /*rval*/) { }
282 
283  void
284  flushWritebuff (const bool force_cache_dealloc);
285 
286  /** \brief Name of the storage file on disk (i.e., the PCD file) */
287  std::string disk_storage_filename_;
288 
289  //--- possibly deprecated parameter variables --//
290 
291  //number of elements in file
292  std::uint64_t filelen_;
293 
294  /** \brief elements [0,...,size()-1] map to [filelen, ..., filelen + size()-1] */
295  AlignedPointTVector writebuff_;
296 
297  const static std::uint64_t READ_BLOCK_SIZE_;
298 
299  static const std::uint64_t WRITE_BUFF_MAX_;
300 
301  static std::mutex rng_mutex_;
302  static boost::mt19937 rand_gen_;
303  static boost::uuids::basic_random_generator<boost::mt19937> uuid_gen_;
304 
305  };
306  } //namespace outofcore
307 } //namespace pcl
pcl::outofcore::OutofcoreOctreeDiskContainer::read
int read(pcl::PCLPointCloud2::Ptr &output_cloud)
Reads the entire point contents from disk into output_cloud.
Definition: octree_disk_container.hpp:576
pcl
Definition: convolution.h:46
pcl::outofcore::OutofcoreOctreeDiskContainer::flush
void flush(const bool force_cache_dealloc)
Exposed functionality for manually flushing the write buffer during tree creation.
Definition: octree_disk_container.h:196
pcl::outofcore::OutofcoreOctreeDiskContainer::operator[]
PointT operator[](std::uint64_t idx) const override
provides random access to points based on a linear index
Definition: octree_disk_container.hpp:198
pcl::outofcore::OutofcoreOctreeDiskContainer::readRangeSubSample
void readRangeSubSample(const std::uint64_t start, const std::uint64_t count, const double percent, AlignedPointTVector &dst) override
grab percent*count random points.
Definition: octree_disk_container.hpp:358
pcl::PCLPointCloud2::Ptr
shared_ptr< ::pcl::PCLPointCloud2 > Ptr
Definition: PCLPointCloud2.h:34
pcl::outofcore::OutofcoreOctreeDiskContainer::size
std::uint64_t size() const override
Returns the total number of points for which this container is responsible, filelen_ + points in writ...
Definition: octree_disk_container.h:181
pcl::outofcore::OutofcoreOctreeDiskContainer::OutofcoreOctreeDiskContainer
OutofcoreOctreeDiskContainer()
Empty constructor creates disk container and sets filename from random uuid string.
Definition: octree_disk_container.hpp:104
pcl::outofcore::OutofcoreAbstractNodeContainer::AlignedPointTVector
std::vector< PointT, Eigen::aligned_allocator< PointT > > AlignedPointTVector
Definition: octree_abstract_node_container.h:56
pcl::outofcore::OutofcoreOctreeDiskContainer::readRangeSubSample_bernoulli
void readRangeSubSample_bernoulli(const std::uint64_t start, const std::uint64_t count, const double percent, AlignedPointTVector &dst)
Use bernoulli trials to select points.
Definition: octree_disk_container.hpp:265
pcl::PointXYZRGB
A point structure representing Euclidean xyz coordinates, and the RGB color.
Definition: point_types.hpp:629
pcl::outofcore::OutofcoreOctreeDiskContainer::readRange
void readRange(const std::uint64_t start, const std::uint64_t count, AlignedPointTVector &dst) override
Reads count points into memory from the disk container.
Definition: octree_disk_container.hpp:239
pcl::outofcore::OutofcoreOctreeDiskContainer::empty
bool empty() const override
STL-like empty test.
Definition: octree_disk_container.h:189
pcl::outofcore::OutofcoreOctreeDiskContainer::getRandomUUIDString
static void getRandomUUIDString(std::string &s)
Generate a universally unique identifier (UUID)
Definition: octree_disk_container.hpp:89
pcl::outofcore::OutofcoreOctreeDiskContainer::path
std::string & path()
Returns this objects path name.
Definition: octree_disk_container.h:203
pcl::outofcore::OutofcoreOctreeDiskContainer::~OutofcoreOctreeDiskContainer
~OutofcoreOctreeDiskContainer()
flushes write buffer, then frees memory
Definition: octree_disk_container.hpp:160
pcl::outofcore::OutofcoreOctreeDiskContainer
Class responsible for serialization and deserialization of out of core point data.
Definition: octree_disk_container.h:78
pcl::outofcore::OutofcoreOctreeDiskContainer::AlignedPointTVector
typename OutofcoreAbstractNodeContainer< PointT >::AlignedPointTVector AlignedPointTVector
Definition: octree_disk_container.h:81
pcl::outofcore::OutofcoreOctreeDiskContainer::push_back
void push_back(const PointT &p)
Adds a single point to the buffer to be written to disk when the buffer grows sufficiently large,...
Definition: octree_disk_container.hpp:455
pcl::outofcore::OutofcoreOctreeDiskContainer::convertToXYZ
void convertToXYZ(const boost::filesystem::path &path) override
write points to disk as ascii
Definition: octree_disk_container.h:225
pcl::outofcore::OutofcoreAbstractNodeContainer
Definition: octree_abstract_node_container.h:53
pcl::utils::ignore
void ignore(const T &...)
Utility function to eliminate unused variable warnings.
Definition: utils.h:62
pcl::outofcore::OutofcoreOctreeDiskContainer::clear
void clear() override
Definition: octree_disk_container.h:209
pcl::outofcore::OutofcoreOctreeDiskContainer::getDataSize
std::uint64_t getDataSize() const
Returns the number of points in the PCD file by reading the PCD header.
Definition: octree_disk_container.hpp:672
pcl::outofcore::OutofcoreOctreeDiskContainer::insertRange
void insertRange(const AlignedPointTVector &src)
Inserts a vector of points into the disk data structure.
Definition: octree_disk_container.hpp:466
pcl::uint64_t
std::uint64_t uint64_t
Definition: types.h:60