Point Cloud Library (PCL)  1.12.1-dev
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 <string>
45 
46 // Boost
47 #include <boost/uuid/random_generator.hpp>
48 
49 #include <pcl/common/utils.h> // pcl::utils::ignore
50 #include <pcl/outofcore/boost.h>
51 #include <pcl/outofcore/octree_abstract_node_container.h>
52 #include <pcl/io/pcd_io.h>
53 #include <pcl/PCLPointCloud2.h>
54 
55 //allows operation on POSIX
56 #if !defined _WIN32
57 #define _fseeki64 fseeko
58 #elif defined __MINGW32__
59 #define _fseeki64 fseeko64
60 #endif
61 
62 namespace pcl
63 {
64  namespace outofcore
65  {
66  /** \class OutofcoreOctreeDiskContainer
67  * \note Code was adapted from the Urban Robotics out of core octree implementation.
68  * Contact Jacob Schloss <jacob.schloss@urbanrobotics.net> with any questions.
69  * http://www.urbanrobotics.net/
70  *
71  * \brief Class responsible for serialization and deserialization of out of core point data
72  * \ingroup outofcore
73  * \author Jacob Schloss (jacob.schloss@urbanrobotics.net)
74  */
75  template<typename PointT = pcl::PointXYZ>
77  {
78 
79  public:
81 
82  /** \brief Empty constructor creates disk container and sets filename from random uuid string*/
84 
85  /** \brief Creates uuid named file or loads existing file
86  *
87  * If \b dir is a directory, this constructor will create a new
88  * uuid named file; if \b dir is an existing file, it will load the
89  * file metadata for accessing the tree.
90  *
91  * \param[in] dir Path to the tree. If it is a directory, it
92  * will create the metadata. If it is a file, it will load the metadata into memory.
93  */
94  OutofcoreOctreeDiskContainer (const boost::filesystem::path &dir);
95 
96  /** \brief flushes write buffer, then frees memory */
98 
99  /** \brief provides random access to points based on a linear index
100  */
101  inline PointT
102  operator[] (std::uint64_t idx) const override;
103 
104  /** \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 */
105  inline void
106  push_back (const PointT& p);
107 
108  /** \brief Inserts a vector of points into the disk data structure */
109  void
110  insertRange (const AlignedPointTVector& src);
111 
112  /** \brief Inserts a PCLPointCloud2 object directly into the disk container */
113  void
114  insertRange (const pcl::PCLPointCloud2::Ptr &input_cloud);
115 
116  void
117  insertRange (const PointT* const * start, const std::uint64_t count) override;
118 
119  /** \brief This is the primary method for serialization of
120  * blocks of point data. This is called by the outofcore
121  * octree interface, opens the binary file for appending data,
122  * and writes it to disk.
123  *
124  * \param[in] start address of the first point to insert
125  * \param[in] count offset from start of the last point to insert
126  */
127  void
128  insertRange (const PointT* start, const std::uint64_t count) override;
129 
130  /** \brief Reads \b count points into memory from the disk container
131  *
132  * Reads \b count points into memory from the disk container, reading at most 2 million elements at a time
133  *
134  * \param[in] start index of first point to read from disk
135  * \param[in] count offset of last point to read from disk
136  * \param[out] dst std::vector as destination for points read from disk into memory
137  */
138  void
139  readRange (const std::uint64_t start, const std::uint64_t count, AlignedPointTVector &dst) override;
140 
141  void
142  readRange (const std::uint64_t, const std::uint64_t, pcl::PCLPointCloud2::Ptr &dst);
143 
144  /** \brief Reads the entire point contents from disk into \c output_cloud
145  * \param[out] output_cloud
146  */
147  int
148  read (pcl::PCLPointCloud2::Ptr &output_cloud);
149 
150  /** \brief grab percent*count random points. points are \b not guaranteed to be
151  * unique (could have multiple identical points!)
152  *
153  * \param[in] start The starting index of points to select
154  * \param[in] count The length of the range of points from which to randomly sample
155  * (i.e. from start to start+count)
156  * \param[in] percent The percentage of count that is enough points to make up this random sample
157  * \param[out] dst std::vector as destination for randomly sampled points; size will
158  * be percentage*count
159  */
160  void
161  readRangeSubSample (const std::uint64_t start, const std::uint64_t count, const double percent,
162  AlignedPointTVector &dst) override;
163 
164  /** \brief Use bernoulli trials to select points. All points selected will be unique.
165  *
166  * \param[in] start The starting index of points to select
167  * \param[in] count The length of the range of points from which to randomly sample
168  * (i.e. from start to start+count)
169  * \param[in] percent The percentage of count that is enough points to make up this random sample
170  * \param[out] dst std::vector as destination for randomly sampled points; size will
171  * be percentage*count
172  */
173  void
174  readRangeSubSample_bernoulli (const std::uint64_t start, const std::uint64_t count,
175  const double percent, AlignedPointTVector& dst);
176 
177  /** \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
178  */
179  std::uint64_t
180  size () const override
181  {
182  return (filelen_ + writebuff_.size ());
183  }
184 
185  /** \brief STL-like empty test
186  * \return true if container has no data on disk or waiting to be written in \c writebuff_ */
187  inline bool
188  empty () const override
189  {
190  return ((filelen_ == 0) && writebuff_.empty ());
191  }
192 
193  /** \brief Exposed functionality for manually flushing the write buffer during tree creation */
194  void
195  flush (const bool force_cache_dealloc)
196  {
197  flushWritebuff (force_cache_dealloc);
198  }
199 
200  /** \brief Returns this objects path name */
201  inline std::string&
202  path ()
203  {
204  return (disk_storage_filename_);
205  }
206 
207  inline void
208  clear () override
209  {
210  //clear elements that have not yet been written to disk
211  writebuff_.clear ();
212  //remove the binary data in the directory
213  PCL_DEBUG ("[Octree Disk Container] Removing the point data from disk, in file %s\n", disk_storage_filename_.c_str ());
214  boost::filesystem::remove (boost::filesystem::path (disk_storage_filename_.c_str ()));
215  //reset the size-of-file counter
216  filelen_ = 0;
217  }
218 
219  /** \brief write points to disk as ascii
220  *
221  * \param[in] path
222  */
223  void
224  convertToXYZ (const boost::filesystem::path &path) override
225  {
226  if (boost::filesystem::exists (disk_storage_filename_))
227  {
228  FILE* fxyz = fopen (path.string ().c_str (), "we");
229 
230  FILE* f = fopen (disk_storage_filename_.c_str (), "rbe");
231  assert (f != NULL);
232 
233  std::uint64_t num = size ();
234  PointT p;
235  char* loc = reinterpret_cast<char*> ( &p );
236 
237  for (std::uint64_t i = 0; i < num; i++)
238  {
239  int seekret = _fseeki64 (f, i * sizeof (PointT), SEEK_SET);
240  pcl::utils::ignore(seekret);
241  assert (seekret == 0);
242  std::size_t readlen = fread (loc, sizeof (PointT), 1, f);
243  pcl::utils::ignore(readlen);
244  assert (readlen == 1);
245 
246  //of << p.x << "\t" << p.y << "\t" << p.z << "\n";
247  std::stringstream ss;
248  ss << std::fixed;
249  ss.precision (16);
250  ss << p.x << "\t" << p.y << "\t" << p.z << "\n";
251 
252  fwrite (ss.str ().c_str (), 1, ss.str ().size (), fxyz);
253  }
254  int res = fclose (f);
255  pcl::utils::ignore(res);
256  assert (res == 0);
257  res = fclose (fxyz);
258  assert (res == 0);
259  }
260  }
261 
262  /** \brief Generate a universally unique identifier (UUID)
263  *
264  * A mutex lock happens to ensure uniqueness
265  *
266  */
267  static void
268  getRandomUUIDString (std::string &s);
269 
270  /** \brief Returns the number of points in the PCD file by reading the PCD header. */
271  std::uint64_t
272  getDataSize () const;
273 
274  private:
275  //no copy construction
277 
278 
280  operator= (const OutofcoreOctreeDiskContainer& /*rval*/) { }
281 
282  void
283  flushWritebuff (const bool force_cache_dealloc);
284 
285  /** \brief Name of the storage file on disk (i.e., the PCD file) */
286  std::string disk_storage_filename_;
287 
288  //--- possibly deprecated parameter variables --//
289 
290  //number of elements in file
291  std::uint64_t filelen_;
292 
293  /** \brief elements [0,...,size()-1] map to [filelen, ..., filelen + size()-1] */
294  AlignedPointTVector writebuff_;
295 
296  const static std::uint64_t READ_BLOCK_SIZE_;
297 
298  static const std::uint64_t WRITE_BUFF_MAX_;
299 
300  static std::mutex rng_mutex_;
301  static boost::mt19937 rand_gen_;
302  static boost::uuids::basic_random_generator<boost::mt19937> uuid_gen_;
303 
304  };
305  } //namespace outofcore
306 } //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:195
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:35
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:180
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:55
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:678
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:188
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:202
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:76
pcl::outofcore::OutofcoreOctreeDiskContainer::AlignedPointTVector
typename OutofcoreAbstractNodeContainer< PointT >::AlignedPointTVector AlignedPointTVector
Definition: octree_disk_container.h:80
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:224
pcl::outofcore::OutofcoreAbstractNodeContainer
Definition: octree_abstract_node_container.h:51
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:208
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