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