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.hpp 6927M 2012-08-24 17:01:57Z (local) $ 00038 */ 00039 00040 #ifndef PCL_OUTOFCORE_OCTREE_DISK_CONTAINER_IMPL_H_ 00041 #define PCL_OUTOFCORE_OCTREE_DISK_CONTAINER_IMPL_H_ 00042 00043 // C++ 00044 #include <sstream> 00045 #include <cassert> 00046 #include <ctime> 00047 00048 // Boost 00049 #include <pcl/outofcore/boost.h> 00050 00051 // PCL 00052 #include <pcl/io/pcd_io.h> 00053 #include <pcl/point_types.h> 00054 #include <pcl/PCLPointCloud2.h> 00055 00056 // PCL (Urban Robotics) 00057 #include <pcl/outofcore/octree_disk_container.h> 00058 00059 //allows operation on POSIX 00060 #if !defined WIN32 00061 #define _fseeki64 fseeko 00062 #elif defined __MINGW32__ 00063 #define _fseeki64 fseeko64 00064 #endif 00065 00066 namespace pcl 00067 { 00068 namespace outofcore 00069 { 00070 template<typename PointT> 00071 boost::mutex OutofcoreOctreeDiskContainer<PointT>::rng_mutex_; 00072 00073 template<typename PointT> boost::mt19937 00074 OutofcoreOctreeDiskContainer<PointT>::rand_gen_ (static_cast<unsigned int> (std::time(NULL))); 00075 00076 template<typename PointT> 00077 boost::uuids::random_generator OutofcoreOctreeDiskContainer<PointT>::uuid_gen_ (&rand_gen_); 00078 00079 template<typename PointT> 00080 const uint64_t OutofcoreOctreeDiskContainer<PointT>::READ_BLOCK_SIZE_ = static_cast<uint64_t> (2e12); 00081 template<typename PointT> 00082 const uint64_t OutofcoreOctreeDiskContainer<PointT>::WRITE_BUFF_MAX_ = static_cast<uint64_t> (2e12); 00083 00084 template<typename PointT> void 00085 OutofcoreOctreeDiskContainer<PointT>::getRandomUUIDString (std::string& s) 00086 { 00087 boost::uuids::uuid u; 00088 { 00089 boost::mutex::scoped_lock lock (rng_mutex_); 00090 u = uuid_gen_ (); 00091 } 00092 00093 std::stringstream ss; 00094 ss << u; 00095 s = ss.str (); 00096 } 00097 //////////////////////////////////////////////////////////////////////////////// 00098 00099 template<typename PointT> 00100 OutofcoreOctreeDiskContainer<PointT>::OutofcoreOctreeDiskContainer () 00101 : disk_storage_filename_ () 00102 , filelen_ (0) 00103 , writebuff_ (0) 00104 { 00105 std::string temp; 00106 getRandomUUIDString (temp); 00107 disk_storage_filename_ = boost::shared_ptr<std::string> (new std::string (temp)); 00108 filelen_ = 0; 00109 } 00110 //////////////////////////////////////////////////////////////////////////////// 00111 00112 template<typename PointT> 00113 OutofcoreOctreeDiskContainer<PointT>::OutofcoreOctreeDiskContainer (const boost::filesystem::path& path) 00114 : disk_storage_filename_ () 00115 , filelen_ (0) 00116 , writebuff_ (0) 00117 { 00118 if (boost::filesystem::exists (path)) 00119 { 00120 if (boost::filesystem::is_directory (path)) 00121 { 00122 std::string uuid; 00123 getRandomUUIDString (uuid); 00124 boost::filesystem::path filename (uuid); 00125 boost::filesystem::path file = path / filename; 00126 00127 disk_storage_filename_ = boost::shared_ptr<std::string> (new std::string (file.string ())); 00128 } 00129 else 00130 { 00131 uint64_t len = boost::filesystem::file_size (path); 00132 00133 disk_storage_filename_ = boost::shared_ptr<std::string> (new std::string (path.string ())); 00134 00135 filelen_ = len / sizeof(PointT); 00136 00137 pcl::PCLPointCloud2 cloud_info; 00138 Eigen::Vector4f origin; 00139 Eigen::Quaternionf orientation; 00140 int pcd_version; 00141 int data_type; 00142 unsigned int data_index; 00143 00144 //read the header of the pcd file and get the number of points 00145 PCDReader reader; 00146 reader.readHeader (*disk_storage_filename_, cloud_info, origin, orientation, pcd_version, data_type, data_index, 0); 00147 00148 filelen_ = cloud_info.width * cloud_info.height; 00149 } 00150 } 00151 else //path doesn't exist 00152 { 00153 disk_storage_filename_ = boost::shared_ptr<std::string> (new std::string (path.string ())); 00154 filelen_ = 0; 00155 } 00156 } 00157 //////////////////////////////////////////////////////////////////////////////// 00158 00159 template<typename PointT> 00160 OutofcoreOctreeDiskContainer<PointT>::~OutofcoreOctreeDiskContainer () 00161 { 00162 flushWritebuff (true); 00163 } 00164 //////////////////////////////////////////////////////////////////////////////// 00165 00166 template<typename PointT> void 00167 OutofcoreOctreeDiskContainer<PointT>::flushWritebuff (const bool force_cache_dealloc) 00168 { 00169 if (writebuff_.size () > 0) 00170 { 00171 //construct the point cloud for this node 00172 typename pcl::PointCloud<PointT>::Ptr cloud (new pcl::PointCloud<PointT>); 00173 00174 cloud->width = static_cast<uint32_t> (writebuff_.size ()); 00175 cloud->height = 1; 00176 00177 cloud->points = writebuff_; 00178 00179 //write data to a pcd file 00180 pcl::PCDWriter writer; 00181 00182 00183 PCL_WARN ("[pcl::outofcore::OutofcoreOctreeDiskContainer::%s] Flushing writebuffer in a dangerous way to file %s. This might overwrite data in destination file\n", __FUNCTION__, disk_storage_filename_->c_str ()); 00184 00185 // Write ascii for now to debug 00186 int res = writer.writeBinaryCompressed (*disk_storage_filename_, *cloud); 00187 (void)res; 00188 assert (res == 0); 00189 if (force_cache_dealloc) 00190 { 00191 writebuff_.resize (0); 00192 } 00193 } 00194 } 00195 //////////////////////////////////////////////////////////////////////////////// 00196 00197 template<typename PointT> PointT 00198 OutofcoreOctreeDiskContainer<PointT>::operator[] (uint64_t idx) const 00199 { 00200 PCL_THROW_EXCEPTION (PCLException, "[pcl::outofcore::OutofcoreOctreeDiskContainer] Not implemented for use with PCL library\n"); 00201 00202 //if the index is on disk 00203 if (idx < filelen_) 00204 { 00205 00206 PointT temp; 00207 //open our file 00208 FILE* f = fopen (disk_storage_filename_->c_str (), "rb"); 00209 assert (f != NULL); 00210 00211 //seek the right length; 00212 int seekret = _fseeki64 (f, idx * sizeof(PointT), SEEK_SET); 00213 (void)seekret; 00214 assert (seekret == 0); 00215 00216 size_t readlen = fread (&temp, 1, sizeof(PointT), f); 00217 (void)readlen; 00218 assert (readlen == sizeof (PointT)); 00219 00220 int res = fclose (f); 00221 (void)res; 00222 assert (res == 0); 00223 00224 return (temp); 00225 } 00226 //otherwise if the index is still in the write buffer 00227 if (idx < (filelen_ + writebuff_.size ())) 00228 { 00229 idx -= filelen_; 00230 return (writebuff_[idx]); 00231 } 00232 00233 //else, throw out of range exception 00234 PCL_THROW_EXCEPTION (PCLException, "[pcl::outofcore:OutofcoreOctreeDiskContainer] Index is out of range"); 00235 } 00236 00237 //////////////////////////////////////////////////////////////////////////////// 00238 template<typename PointT> void 00239 OutofcoreOctreeDiskContainer<PointT>::readRange (const uint64_t start, const uint64_t count, AlignedPointTVector& dst) 00240 { 00241 if (count == 0) 00242 { 00243 return; 00244 } 00245 00246 if ((start + count) > size ()) 00247 { 00248 PCL_ERROR ("[pcl::outofcore::OutofcoreOctreeDiskContainer::%s] Indicies out of range; start + count exceeds the size of the stored points\n", __FUNCTION__); 00249 PCL_THROW_EXCEPTION (PCLException, "[pcl::outofcore::OutofcoreOctreeDiskContainer] Outofcore Octree Exception: Read indices exceed range"); 00250 } 00251 00252 pcl::PCDReader reader; 00253 typename pcl::PointCloud<PointT>::Ptr cloud (new pcl::PointCloud<PointT> ()); 00254 00255 int res = reader.read (*disk_storage_filename_, *cloud); 00256 (void)res; 00257 assert (res == 0); 00258 00259 for (size_t i=0; i < cloud->points.size (); i++) 00260 dst.push_back (cloud->points[i]); 00261 00262 } 00263 //////////////////////////////////////////////////////////////////////////////// 00264 00265 template<typename PointT> void 00266 OutofcoreOctreeDiskContainer<PointT>::readRangeSubSample_bernoulli (const uint64_t start, const uint64_t count, const double percent, AlignedPointTVector& dst) 00267 { 00268 if (count == 0) 00269 { 00270 return; 00271 } 00272 00273 dst.clear (); 00274 00275 uint64_t filestart = 0; 00276 uint64_t filecount = 0; 00277 00278 int64_t buffstart = -1; 00279 int64_t buffcount = -1; 00280 00281 if (start < filelen_) 00282 { 00283 filestart = start; 00284 } 00285 00286 if ((start + count) <= filelen_) 00287 { 00288 filecount = count; 00289 } 00290 else 00291 { 00292 filecount = filelen_ - start; 00293 00294 buffstart = 0; 00295 buffcount = count - filecount; 00296 } 00297 00298 if (buffcount > 0) 00299 { 00300 { 00301 boost::mutex::scoped_lock lock (rng_mutex_); 00302 boost::bernoulli_distribution<double> buffdist (percent); 00303 boost::variate_generator<boost::mt19937&, boost::bernoulli_distribution<double> > buffcoin (rand_gen_, buffdist); 00304 00305 for (size_t i = buffstart; i < static_cast<uint64_t> (buffcount); i++) 00306 { 00307 if (buffcoin ()) 00308 { 00309 dst.push_back (writebuff_[i]); 00310 } 00311 } 00312 } 00313 } 00314 00315 if (filecount > 0) 00316 { 00317 //pregen and then sort the offsets to reduce the amount of seek 00318 std::vector < uint64_t > offsets; 00319 { 00320 boost::mutex::scoped_lock lock (rng_mutex_); 00321 00322 boost::bernoulli_distribution<double> filedist (percent); 00323 boost::variate_generator<boost::mt19937&, boost::bernoulli_distribution<double> > filecoin (rand_gen_, filedist); 00324 for (uint64_t i = filestart; i < (filestart + filecount); i++) 00325 { 00326 if (filecoin ()) 00327 { 00328 offsets.push_back (i); 00329 } 00330 } 00331 } 00332 std::sort (offsets.begin (), offsets.end ()); 00333 00334 FILE* f = fopen (disk_storage_filename_->c_str (), "rb"); 00335 assert (f != NULL); 00336 PointT p; 00337 char* loc = reinterpret_cast<char*> (&p); 00338 00339 uint64_t filesamp = offsets.size (); 00340 for (uint64_t i = 0; i < filesamp; i++) 00341 { 00342 int seekret = _fseeki64 (f, offsets[i] * static_cast<uint64_t> (sizeof(PointT)), SEEK_SET); 00343 (void)seekret; 00344 assert (seekret == 0); 00345 size_t readlen = fread (loc, sizeof(PointT), 1, f); 00346 (void)readlen; 00347 assert (readlen == 1); 00348 00349 dst.push_back (p); 00350 } 00351 00352 fclose (f); 00353 } 00354 } 00355 //////////////////////////////////////////////////////////////////////////////// 00356 00357 //change this to use a weighted coin flip, to allow sparse sampling of small clouds (eg the bernoulli above) 00358 template<typename PointT> void 00359 OutofcoreOctreeDiskContainer<PointT>::readRangeSubSample (const uint64_t start, const uint64_t count, const double percent, AlignedPointTVector& dst) 00360 { 00361 if (count == 0) 00362 { 00363 return; 00364 } 00365 00366 dst.clear (); 00367 00368 uint64_t filestart = 0; 00369 uint64_t filecount = 0; 00370 00371 int64_t buffcount = -1; 00372 00373 if (start < filelen_) 00374 { 00375 filestart = start; 00376 } 00377 00378 if ((start + count) <= filelen_) 00379 { 00380 filecount = count; 00381 } 00382 else 00383 { 00384 filecount = filelen_ - start; 00385 buffcount = count - filecount; 00386 } 00387 00388 uint64_t filesamp = static_cast<uint64_t> (percent * static_cast<double> (filecount)); 00389 00390 uint64_t buffsamp = (buffcount > 0) ? (static_cast<uint64_t > (percent * static_cast<double> (buffcount))) : 0; 00391 00392 if ((filesamp == 0) && (buffsamp == 0) && (size () > 0)) 00393 { 00394 //std::cerr << "would not add points to LOD, falling back to bernoulli"; 00395 readRangeSubSample_bernoulli (start, count, percent, dst); 00396 return; 00397 } 00398 00399 if (buffcount > 0) 00400 { 00401 { 00402 boost::mutex::scoped_lock lock (rng_mutex_); 00403 00404 boost::uniform_int < uint64_t > buffdist (0, buffcount - 1); 00405 boost::variate_generator<boost::mt19937&, boost::uniform_int<uint64_t> > buffdie (rand_gen_, buffdist); 00406 00407 for (uint64_t i = 0; i < buffsamp; i++) 00408 { 00409 uint64_t buffstart = buffdie (); 00410 dst.push_back (writebuff_[buffstart]); 00411 } 00412 } 00413 } 00414 00415 if (filesamp > 0) 00416 { 00417 //pregen and then sort the offsets to reduce the amount of seek 00418 std::vector < uint64_t > offsets; 00419 { 00420 boost::mutex::scoped_lock lock (rng_mutex_); 00421 00422 offsets.resize (filesamp); 00423 boost::uniform_int < uint64_t > filedist (filestart, filestart + filecount - 1); 00424 boost::variate_generator<boost::mt19937&, boost::uniform_int<uint64_t> > filedie (rand_gen_, filedist); 00425 for (uint64_t i = 0; i < filesamp; i++) 00426 { 00427 uint64_t _filestart = filedie (); 00428 offsets[i] = _filestart; 00429 } 00430 } 00431 std::sort (offsets.begin (), offsets.end ()); 00432 00433 FILE* f = fopen (disk_storage_filename_->c_str (), "rb"); 00434 assert (f != NULL); 00435 PointT p; 00436 char* loc = reinterpret_cast<char*> (&p); 00437 for (uint64_t i = 0; i < filesamp; i++) 00438 { 00439 int seekret = _fseeki64 (f, offsets[i] * static_cast<uint64_t> (sizeof(PointT)), SEEK_SET); 00440 (void)seekret; 00441 assert (seekret == 0); 00442 size_t readlen = fread (loc, sizeof(PointT), 1, f); 00443 (void)readlen; 00444 assert (readlen == 1); 00445 00446 dst.push_back (p); 00447 } 00448 int res = fclose (f); 00449 (void)res; 00450 assert (res == 0); 00451 } 00452 } 00453 //////////////////////////////////////////////////////////////////////////////// 00454 00455 template<typename PointT> void 00456 OutofcoreOctreeDiskContainer<PointT>::push_back (const PointT& p) 00457 { 00458 writebuff_.push_back (p); 00459 if (writebuff_.size () > WRITE_BUFF_MAX_) 00460 { 00461 flushWritebuff (false); 00462 } 00463 } 00464 //////////////////////////////////////////////////////////////////////////////// 00465 00466 template<typename PointT> void 00467 OutofcoreOctreeDiskContainer<PointT>::insertRange (const AlignedPointTVector& src) 00468 { 00469 const uint64_t count = src.size (); 00470 00471 typename pcl::PointCloud<PointT>::Ptr tmp_cloud (new pcl::PointCloud<PointT> ()); 00472 00473 // If there's a pcd file with data 00474 if (boost::filesystem::exists (*disk_storage_filename_)) 00475 { 00476 // Open the existing file 00477 pcl::PCDReader reader; 00478 int res = reader.read (*disk_storage_filename_, *tmp_cloud); 00479 (void)res; 00480 assert (res == 0); 00481 } 00482 // Otherwise create the point cloud which will be saved to the pcd file for the first time 00483 else 00484 { 00485 tmp_cloud->width = static_cast<uint32_t> (count + writebuff_.size ()); 00486 tmp_cloud->height = 1; 00487 } 00488 00489 for (size_t i = 0; i < src.size (); i++) 00490 tmp_cloud->points.push_back (src[i]); 00491 00492 // If there are any points in the write cache writebuff_, a different write cache than this one, concatenate 00493 for (size_t i = 0; i < writebuff_.size (); i++) 00494 { 00495 tmp_cloud->points.push_back (writebuff_[i]); 00496 } 00497 00498 //assume unorganized point cloud 00499 tmp_cloud->width = static_cast<uint32_t> (tmp_cloud->points.size ()); 00500 00501 //save and close 00502 PCDWriter writer; 00503 00504 int res = writer.writeBinaryCompressed (*disk_storage_filename_, *tmp_cloud); 00505 (void)res; 00506 assert (res == 0); 00507 } 00508 00509 //////////////////////////////////////////////////////////////////////////////// 00510 00511 template<typename PointT> void 00512 OutofcoreOctreeDiskContainer<PointT>::insertRange (const pcl::PCLPointCloud2::Ptr& input_cloud) 00513 { 00514 pcl::PCLPointCloud2::Ptr tmp_cloud (new pcl::PCLPointCloud2 ()); 00515 00516 //if there's a pcd file with data associated with this node, read the data, concatenate, and resave 00517 if (boost::filesystem::exists (*disk_storage_filename_)) 00518 { 00519 //open the existing file 00520 pcl::PCDReader reader; 00521 int res = reader.read (*disk_storage_filename_, *tmp_cloud); 00522 (void)res; 00523 assert (res == 0); 00524 pcl::PCDWriter writer; 00525 PCL_DEBUG ("[pcl::outofcore::OutofcoreOctreeDiskContainer::%s] Concatenating point cloud from %s to new cloud\n", __FUNCTION__, disk_storage_filename_->c_str ()); 00526 00527 size_t previous_num_pts = tmp_cloud->width*tmp_cloud->height + input_cloud->width*input_cloud->height; 00528 //Concatenate will fail if the fields in input_cloud do not match the fields in the PCD file. 00529 pcl::concatenatePointCloud (*tmp_cloud, *input_cloud, *tmp_cloud); 00530 size_t res_pts = tmp_cloud->width*tmp_cloud->height; 00531 00532 (void)previous_num_pts; 00533 (void)res_pts; 00534 00535 assert (previous_num_pts == res_pts); 00536 00537 writer.writeBinaryCompressed (*disk_storage_filename_, *tmp_cloud); 00538 00539 } 00540 else //otherwise create the point cloud which will be saved to the pcd file for the first time 00541 { 00542 pcl::PCDWriter writer; 00543 int res = writer.writeBinaryCompressed (*disk_storage_filename_, *input_cloud); 00544 (void)res; 00545 assert (res == 0); 00546 } 00547 00548 } 00549 00550 //////////////////////////////////////////////////////////////////////////////// 00551 00552 template<typename PointT> void 00553 OutofcoreOctreeDiskContainer<PointT>::readRange (const uint64_t, const uint64_t, pcl::PCLPointCloud2::Ptr& dst) 00554 { 00555 pcl::PCDReader reader; 00556 00557 Eigen::Vector4f origin; 00558 Eigen::Quaternionf orientation; 00559 int pcd_version; 00560 00561 if (boost::filesystem::exists (*disk_storage_filename_)) 00562 { 00563 // PCL_INFO ("[pcl::outofcore::OutofcoreOctreeDiskContainer::%s] Reading points from disk from %s.\n", __FUNCTION__ , disk_storage_filename_->c_str ()); 00564 int res = reader.read (*disk_storage_filename_, *dst, origin, orientation, pcd_version); 00565 (void)res; 00566 assert (res != -1); 00567 } 00568 else 00569 { 00570 PCL_ERROR ("[pcl::outofcore::OutofcoreOctreeDiskContainer::%s] File %s does not exist in node.\n", __FUNCTION__, disk_storage_filename_->c_str ()); 00571 } 00572 } 00573 00574 //////////////////////////////////////////////////////////////////////////////// 00575 00576 template<typename PointT> int 00577 OutofcoreOctreeDiskContainer<PointT>::read (pcl::PCLPointCloud2::Ptr& output_cloud) 00578 { 00579 pcl::PCLPointCloud2::Ptr temp_output_cloud (new pcl::PCLPointCloud2 ()); 00580 00581 if (boost::filesystem::exists (*disk_storage_filename_)) 00582 { 00583 // PCL_INFO ("[pcl::outofcore::OutofcoreOctreeDiskContainer::%s] Reading points from disk from %s.\n", __FUNCTION__ , disk_storage_filename_->c_str ()); 00584 int res = pcl::io::loadPCDFile (*disk_storage_filename_, *temp_output_cloud); 00585 (void)res; 00586 assert (res != -1); 00587 if(res == -1) 00588 return (-1); 00589 } 00590 else 00591 { 00592 PCL_ERROR ("[pcl::outofcore::OutofcoreOctreeDiskContainer::%s] File %s does not exist in node.\n", __FUNCTION__, disk_storage_filename_->c_str ()); 00593 return (-1); 00594 } 00595 00596 if(output_cloud.get () != 0) 00597 { 00598 pcl::concatenatePointCloud (*output_cloud, *temp_output_cloud, *output_cloud); 00599 } 00600 else 00601 { 00602 output_cloud = temp_output_cloud; 00603 } 00604 return (0); 00605 } 00606 00607 //////////////////////////////////////////////////////////////////////////////// 00608 00609 template<typename PointT> void 00610 OutofcoreOctreeDiskContainer<PointT>::insertRange (const PointT* const * start, const uint64_t count) 00611 { 00612 // PCL_THROW_EXCEPTION (PCLException, "[pcl::outofcore::OutofcoreOctreeDiskContainer] Deprecated\n"); 00613 //copy the handles to a continuous block 00614 PointT* arr = new PointT[count]; 00615 00616 //copy from start of array, element by element 00617 for (size_t i = 0; i < count; i++) 00618 { 00619 arr[i] = *(start[i]); 00620 } 00621 00622 insertRange (arr, count); 00623 delete[] arr; 00624 } 00625 00626 //////////////////////////////////////////////////////////////////////////////// 00627 00628 template<typename PointT> void 00629 OutofcoreOctreeDiskContainer<PointT>::insertRange (const PointT* start, const uint64_t count) 00630 { 00631 typename pcl::PointCloud<PointT>::Ptr tmp_cloud (new pcl::PointCloud<PointT> ()); 00632 00633 // If there's a pcd file with data, read it in from disk for appending 00634 if (boost::filesystem::exists (*disk_storage_filename_)) 00635 { 00636 pcl::PCDReader reader; 00637 // Open it 00638 int res = reader.read (disk_storage_filename_->c_str (), *tmp_cloud); 00639 (void)res; 00640 assert (res == 0); 00641 } 00642 else //otherwise create the pcd file 00643 { 00644 tmp_cloud->width = static_cast<uint32_t> (count) + static_cast<uint32_t> (writebuff_.size ()); 00645 tmp_cloud->height = 1; 00646 } 00647 00648 // Add any points in the cache 00649 for (size_t i = 0; i < writebuff_.size (); i++) 00650 { 00651 tmp_cloud->points.push_back (writebuff_ [i]); 00652 } 00653 00654 //add the new points passed with this function 00655 for (size_t i = 0; i < count; i++) 00656 { 00657 tmp_cloud->points.push_back (*(start + i)); 00658 } 00659 00660 tmp_cloud->width = static_cast<uint32_t> (tmp_cloud->points.size ()); 00661 tmp_cloud->height = 1; 00662 00663 //save and close 00664 PCDWriter writer; 00665 00666 int res = writer.writeBinaryCompressed (*disk_storage_filename_, *tmp_cloud); 00667 (void)res; 00668 assert (res == 0); 00669 } 00670 //////////////////////////////////////////////////////////////////////////////// 00671 00672 template<typename PointT> boost::uint64_t 00673 OutofcoreOctreeDiskContainer<PointT>::getDataSize () const 00674 { 00675 pcl::PCLPointCloud2 cloud_info; 00676 Eigen::Vector4f origin; 00677 Eigen::Quaternionf orientation; 00678 int pcd_version; 00679 int data_type; 00680 unsigned int data_index; 00681 00682 //read the header of the pcd file and get the number of points 00683 PCDReader reader; 00684 reader.readHeader (*disk_storage_filename_, cloud_info, origin, orientation, pcd_version, data_type, data_index, 0); 00685 00686 boost::uint64_t total_points = cloud_info.width * cloud_info.height + writebuff_.size (); 00687 00688 return (total_points); 00689 } 00690 //////////////////////////////////////////////////////////////////////////////// 00691 00692 }//namespace outofcore 00693 }//namespace pcl 00694 00695 #endif //PCL_OUTOFCORE_OCTREE_DISK_CONTAINER_IMPL_H_