Point Cloud Library (PCL)  1.7.0
/tmp/buildd/pcl-1.7-1.7.0/outofcore/include/pcl/outofcore/octree_disk_container.h
00001 /*
00002  * Software License Agreement (BSD License)
00003  *
00004  *  Point Cloud Library (PCL) - www.pointclouds.org
00005  *  Copyright (c) 2010-2012, Willow Garage, Inc.
00006  *  Copyright (c) 2012, Urban Robotics, Inc.
00007  *
00008  *  All rights reserved.
00009  *
00010  *  Redistribution and use in source and binary forms, with or without
00011  *  modification, are permitted provided that the following conditions
00012  *  are met:
00013  *
00014  *   * Redistributions of source code must retain the above copyright
00015  *     notice, this list of conditions and the following disclaimer.
00016  *   * Redistributions in binary form must reproduce the above
00017  *     copyright notice, this list of conditions and the following
00018  *     disclaimer in the documentation and/or other materials provided
00019  *     with the distribution.
00020  *   * Neither the name of Willow Garage, Inc. nor the names of its
00021  *     contributors may be used to endorse or promote products derived
00022  *     from this software without specific prior written permission.
00023  *
00024  *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
00025  *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
00026  *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
00027  *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
00028  *  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
00029  *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
00030  *  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
00031  *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
00032  *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
00033  *  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
00034  *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
00035  *  POSSIBILITY OF SUCH DAMAGE.
00036  *
00037  *  $Id: octree_disk_container.h 6927M 2012-08-24 13:26:40Z (local) $
00038  */
00039 
00040 #ifndef PCL_OUTOFCORE_OCTREE_DISK_CONTAINER_H_
00041 #define PCL_OUTOFCORE_OCTREE_DISK_CONTAINER_H_
00042 
00043 // C++
00044 #include <vector>
00045 #include <string>
00046 
00047 #include <pcl/outofcore/boost.h>
00048 #include <pcl/outofcore/octree_abstract_node_container.h>
00049 #include <pcl/io/pcd_io.h>
00050 #include <pcl/PCLPointCloud2.h>
00051 
00052 //allows operation on POSIX
00053 #if !defined WIN32
00054 #define _fseeki64 fseeko
00055 #elif defined __MINGW32__
00056 #define _fseeki64 fseeko64
00057 #endif
00058 
00059 namespace pcl
00060 {
00061   namespace outofcore
00062   {
00063   /** \class OutofcoreOctreeDiskContainer
00064    *  \note Code was adapted from the Urban Robotics out of core octree implementation. 
00065    *  Contact Jacob Schloss <jacob.schloss@urbanrobotics.net> with any questions. 
00066    *  http://www.urbanrobotics.net/
00067    *
00068    *  \brief Class responsible for serialization and deserialization of out of core point data
00069    *  \ingroup outofcore
00070    *  \author Jacob Schloss (jacob.schloss@urbanrobotics.net)
00071    */
00072     template<typename PointT = pcl::PointXYZ>
00073     class OutofcoreOctreeDiskContainer : public OutofcoreAbstractNodeContainer<PointT>
00074     {
00075   
00076       public:
00077         typedef typename OutofcoreAbstractNodeContainer<PointT>::AlignedPointTVector AlignedPointTVector;
00078         
00079         /** \brief Empty constructor creates disk container and sets filename from random uuid string*/
00080         OutofcoreOctreeDiskContainer ();
00081 
00082         /** \brief Creates uuid named file or loads existing file
00083          * 
00084          * If \b dir is a directory, this constructor will create a new
00085          * uuid named file; if \b dir is an existing file, it will load the
00086          * file metadata for accessing the tree.
00087          *
00088          * \param[in] dir Path to the tree. If it is a directory, it
00089          * will create the metadata. If it is a file, it will load the metadata into memory.
00090          */
00091         OutofcoreOctreeDiskContainer (const boost::filesystem::path &dir);
00092 
00093         /** \brief flushes write buffer, then frees memory */
00094         ~OutofcoreOctreeDiskContainer ();
00095 
00096         /** \brief provides random access to points based on a linear index
00097          */
00098         inline PointT
00099         operator[] (uint64_t idx) const;
00100 
00101         /** \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 */
00102         inline void
00103         push_back (const PointT& p);
00104 
00105         /** \brief Inserts a vector of points into the disk data structure */
00106         void
00107         insertRange (const AlignedPointTVector& src);
00108 
00109         /** \brief Inserts a PCLPointCloud2 object directly into the disk container */
00110         void
00111         insertRange (const pcl::PCLPointCloud2::Ptr &input_cloud);
00112 
00113         void
00114         insertRange (const PointT* const * start, const uint64_t count);
00115     
00116         /** \brief This is the primary method for serialization of
00117          * blocks of point data. This is called by the outofcore
00118          * octree interface, opens the binary file for appending data,
00119          * and writes it to disk.
00120          *
00121          * \param[in] start address of the first point to insert
00122          * \param[in] count offset from start of the last point to insert
00123          */
00124         void
00125         insertRange (const PointT* start, const uint64_t count);
00126 
00127         /** \brief Reads \b count points into memory from the disk container
00128          *
00129          * Reads \b count points into memory from the disk container, reading at most 2 million elements at a time
00130          *
00131          * \param[in] start index of first point to read from disk
00132          * \param[in] count offset of last point to read from disk
00133          * \param[out] v std::vector as destination for points read from disk into memory
00134          */
00135         void
00136         readRange (const uint64_t start, const uint64_t count, AlignedPointTVector &dst);
00137 
00138         void
00139         readRange (const uint64_t, const uint64_t, pcl::PCLPointCloud2::Ptr &dst);
00140 
00141         /** \brief Reads the entire point contents from disk into \ref output_cloud
00142          *  \param[out] output_cloud
00143          */
00144         int
00145         read (pcl::PCLPointCloud2::Ptr &output_cloud);
00146 
00147         /** \brief  grab percent*count random points. points are \b not guaranteed to be
00148          * unique (could have multiple identical points!)
00149          *
00150          * \param[in] start The starting index of points to select
00151          * \param count[in] The length of the range of points from which to randomly sample 
00152          *  (i.e. from start to start+count)
00153          * \param percent[in] The percentage of count that is enough points to make up this random sample
00154          * \param dst[out] std::vector as destination for randomly sampled points; size will 
00155          * be percentage*count
00156          */
00157         void
00158         readRangeSubSample (const uint64_t start, const uint64_t count, const double percent,
00159                             AlignedPointTVector &dst);
00160 
00161         /** \brief Use bernoulli trials to select points. All points selected will be unique.
00162          *
00163          * \param[in] start The starting index of points to select
00164          * \param[in] count The length of the range of points from which to randomly sample 
00165          *  (i.e. from start to start+count)
00166          * \param[in] percent The percentage of count that is enough points to make up this random sample
00167          * \param[out] dst std::vector as destination for randomly sampled points; size will 
00168          * be percentage*count
00169          */
00170         void
00171         readRangeSubSample_bernoulli (const uint64_t start, const uint64_t count, 
00172                                       const double percent, AlignedPointTVector& dst);
00173 
00174         /** \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
00175          */
00176         uint64_t
00177         size () const
00178         {
00179           return (filelen_ + writebuff_.size ());
00180         }
00181 
00182         /** \brief STL-like empty test
00183          * \return true if container has no data on disk or waiting to be written in \ref writebuff_ */
00184         inline bool
00185         empty () const
00186         {
00187           return ((filelen_ == 0) && writebuff_.empty ());
00188         }
00189 
00190         /** \brief Exposed functionality for manually flushing the write buffer during tree creation */
00191         void
00192         flush (const bool force_cache_dealloc)
00193         {
00194           flushWritebuff (force_cache_dealloc);
00195         }
00196 
00197         /** \brief Returns this objects path name */
00198         inline std::string&
00199         path ()
00200         {
00201           return (*disk_storage_filename_);
00202         }
00203 
00204         inline void
00205         clear ()
00206         {
00207           //clear elements that have not yet been written to disk
00208           writebuff_.clear ();
00209           //remove the binary data in the directory
00210           PCL_DEBUG ("[Octree Disk Container] Removing the point data from disk, in file %s\n",disk_storage_filename_->c_str ());
00211           boost::filesystem::remove (boost::filesystem::path (disk_storage_filename_->c_str ()));
00212           //reset the size-of-file counter
00213           filelen_ = 0;
00214         }
00215 
00216         /** \brief write points to disk as ascii
00217          *
00218          * \param[in] path
00219          */
00220         void
00221         convertToXYZ (const boost::filesystem::path &path)
00222         {
00223           if (boost::filesystem::exists (*disk_storage_filename_))
00224           {
00225             FILE* fxyz = fopen (path.string ().c_str (), "w");
00226 
00227             FILE* f = fopen (disk_storage_filename_->c_str (), "rb");
00228             assert (f != NULL);
00229 
00230             uint64_t num = size ();
00231             PointT p;
00232             char* loc = reinterpret_cast<char*> ( &p );
00233 
00234             for (uint64_t i = 0; i < num; i++)
00235             {
00236               int seekret = _fseeki64 (f, i * sizeof (PointT), SEEK_SET);
00237               (void)seekret;
00238               assert (seekret == 0);
00239               size_t readlen = fread (loc, sizeof (PointT), 1, f);
00240               (void)readlen;
00241               assert (readlen == 1);
00242 
00243               //of << p.x << "\t" << p.y << "\t" << p.z << "\n";
00244               std::stringstream ss;
00245               ss << std::fixed;
00246               ss.precision (16);
00247               ss << p.x << "\t" << p.y << "\t" << p.z << "\n";
00248 
00249               fwrite (ss.str ().c_str (), 1, ss.str ().size (), fxyz);
00250             }
00251             int res = fclose (f);
00252             (void)res;
00253             assert (res == 0);
00254             res = fclose (fxyz);
00255             assert (res == 0);
00256           }
00257         }
00258 
00259         /** \brief Generate a universally unique identifier (UUID)
00260          *
00261          * A mutex lock happens to ensure uniquness
00262          *
00263          */
00264         static void
00265         getRandomUUIDString (std::string &s);
00266 
00267         /** \brief Returns the number of points in the PCD file by reading the PCD header. */
00268         boost::uint64_t
00269         getDataSize () const;
00270         
00271       private:
00272         //no copy construction
00273         OutofcoreOctreeDiskContainer (const OutofcoreOctreeDiskContainer &rval) { }
00274 
00275 
00276         OutofcoreOctreeDiskContainer&
00277         operator= (const OutofcoreOctreeDiskContainer &rval) { }
00278 
00279         void
00280         flushWritebuff (const bool force_cache_dealloc);
00281     
00282         /** \brief Name of the storage file on disk (i.e., the PCD file) */
00283         boost::shared_ptr<std::string> disk_storage_filename_;
00284 
00285         //--- possibly deprecated parameter variables --//
00286 
00287         //number of elements in file
00288         uint64_t filelen_;
00289 
00290         /** \brief elements [0,...,size()-1] map to [filelen, ..., filelen + size()-1] */
00291         AlignedPointTVector writebuff_;
00292 
00293         const static uint64_t READ_BLOCK_SIZE_;
00294 
00295         static const uint64_t WRITE_BUFF_MAX_;
00296 
00297         static boost::mutex rng_mutex_;
00298         static boost::mt19937 rand_gen_;
00299         static boost::uuids::random_generator uuid_gen_;
00300 
00301     };
00302   } //namespace outofcore
00303 } //namespace pcl
00304 
00305 #endif //PCL_OUTOFCORE_OCTREE_DISK_CONTAINER_H_