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
shared_ptr< ::pcl::PCLPointCloud2 > Ptr
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
std::uint64_t uint64_t
Definition: types.h:60
bool empty() const override
STL-like empty test.
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)
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.
void ignore(const T &...)
Utility function to eliminate unused variable warnings.
Definition: utils.h:62
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.
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...