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