Point Cloud Library (PCL)  1.14.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 <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
shared_ptr< ::pcl::PCLPointCloud2 > Ptr
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 size() const override
Returns the total number of points for which this container is responsible, filelen_ + points in writ...
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)
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 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
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 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.
bool empty() const override
STL-like empty test.
A point structure representing Euclidean xyz coordinates, and the RGB color.
std::vector< PointT, Eigen::aligned_allocator< PointT > > AlignedPointTVector
std::uint64_t getDataSize() const
Returns the number of points in the PCD file by reading the PCD header.