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