Point Cloud Library (PCL)
1.7.0
|
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_