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