Point Cloud Library (PCL)  1.9.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 <vector>
45 #include <string>
46 
47 // Boost
48 #include <boost/uuid/random_generator.hpp>
49 
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  (void)seekret;
241  assert (seekret == 0);
242  size_t readlen = fread (loc, sizeof (PointT), 1, f);
243  (void)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  (void)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
std::uint64_t getDataSize() const
Returns the number of points in the PCD file by reading the PCD header.
void readRangeSubSample(const std::uint64_t start, const std::uint64_t count, const double percent, AlignedPointTVector &dst) override
grab percent*count random points.
void convertToXYZ(const boost::filesystem::path &path) override
write points to disk as ascii
bool empty() const override
STL-like empty test.
void insertRange(const AlignedPointTVector &src)
Inserts a vector of points into the disk data structure.
This file defines compatibility wrappers for low level I/O functions.
Definition: convolution.h:45
static void getRandomUUIDString(std::string &s)
Generate a universally unique identifier (UUID)
void flush(const bool force_cache_dealloc)
Exposed functionality for manually flushing the write buffer during tree creation.
std::string & path()
Returns this objects path name.
Class responsible for serialization and deserialization of out of core point data.
typename OutofcoreAbstractNodeContainer< PointT >::AlignedPointTVector AlignedPointTVector
~OutofcoreOctreeDiskContainer()
flushes write buffer, then frees memory
void readRangeSubSample_bernoulli(const std::uint64_t start, const std::uint64_t count, const double percent, AlignedPointTVector &dst)
Use bernoulli trials to select points.
boost::shared_ptr< ::pcl::PCLPointCloud2 > Ptr
PointT operator[](std::uint64_t idx) const override
provides random access to points based on a linear index
void push_back(const PointT &p)
Adds a single point to the buffer to be written to disk when the buffer grows sufficiently large...
OutofcoreOctreeDiskContainer()
Empty constructor creates disk container and sets filename from random uuid string.
void readRange(const std::uint64_t start, const std::uint64_t count, AlignedPointTVector &dst) override
Reads count points into memory from the disk container.
int read(pcl::PCLPointCloud2::Ptr &output_cloud)
Reads the entire point contents from disk into output_cloud.
A point structure representing Euclidean xyz coordinates, and the RGB color.
std::vector< PointT, Eigen::aligned_allocator< PointT > > AlignedPointTVector
std::uint64_t size() const override
Returns the total number of points for which this container is responsible, filelen_ + points in writ...