Point Cloud Library (PCL)  1.7.2
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] dst 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 \c 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[in] count The length of the range of points from which to randomly sample
152  * (i.e. from start to start+count)
153  * \param[in] percent The percentage of count that is enough points to make up this random sample
154  * \param[out] dst 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, \c filelen_ + points in \c 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 \c 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_
void readRangeSubSample(const uint64_t start, const uint64_t count, const double percent, AlignedPointTVector &dst)
grab percent*count random points.
uint64_t size() const
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)
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.
void readRange(const uint64_t start, const uint64_t count, AlignedPointTVector &dst)
Reads count points into memory from the disk container.
PointT operator[](uint64_t idx) const
provides random access to points based on a linear index
boost::shared_ptr< ::pcl::PCLPointCloud2 > Ptr
Class responsible for serialization and deserialization of out of core point data.
~OutofcoreOctreeDiskContainer()
flushes write buffer, then frees memory
void push_back(const PointT &p)
Adds a single point to the buffer to be written to disk when the buffer grows sufficiently large...
boost::uint64_t getDataSize() const
Returns the number of points in the PCD file by reading the PCD header.
OutofcoreOctreeDiskContainer()
Empty constructor creates disk container and sets filename from random uuid string.
void readRangeSubSample_bernoulli(const uint64_t start, const uint64_t count, const double percent, AlignedPointTVector &dst)
Use bernoulli trials to select points.
int read(pcl::PCLPointCloud2::Ptr &output_cloud)
Reads the entire point contents from disk into output_cloud.
bool empty() const
STL-like empty test.
OutofcoreAbstractNodeContainer< PointT >::AlignedPointTVector AlignedPointTVector
A point structure representing Euclidean xyz coordinates, and the RGB color.
std::vector< PointT, Eigen::aligned_allocator< PointT > > AlignedPointTVector
void convertToXYZ(const boost::filesystem::path &path)
write points to disk as ascii