Point Cloud Library (PCL)  1.7.0
octree_ram_container.hpp
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_ram_container.hpp 6927 2012-08-23 02:34:54Z stfox88 $
38  */
39 
40 #ifndef PCL_OUTOFCORE_RAM_CONTAINER_IMPL_H_
41 #define PCL_OUTOFCORE_RAM_CONTAINER_IMPL_H_
42 
43 // C++
44 #include <sstream>
45 
46 // PCL (Urban Robotics)
47 #include <pcl/outofcore/octree_ram_container.h>
48 
49 namespace pcl
50 {
51  namespace outofcore
52  {
53 
54  template<typename PointT>
55  boost::mutex OutofcoreOctreeRamContainer<PointT>::rng_mutex_;
56 
57  template<typename PointT>
58  boost::mt19937 OutofcoreOctreeRamContainer<PointT>::rand_gen_ (static_cast<unsigned int>(std::time( NULL)));
59 
60  template<typename PointT> void
61  OutofcoreOctreeRamContainer<PointT>::convertToXYZ (const boost::filesystem::path& path)
62  {
63  if (!container_.empty ())
64  {
65  FILE* fxyz = fopen (path.string ().c_str (), "w");
66 
67  boost::uint64_t num = size ();
68  for (boost::uint64_t i = 0; i < num; i++)
69  {
70  const PointT& p = container_[i];
71 
72  std::stringstream ss;
73  ss << std::fixed;
74  ss.precision (16);
75  ss << p.x << "\t" << p.y << "\t" << p.z << "\n";
76 
77  fwrite (ss.str ().c_str (), 1, ss.str ().size (), fxyz);
78  }
79 
80  assert ( fclose (fxyz) == 0 );
81  }
82  }
83 
84  ////////////////////////////////////////////////////////////////////////////////
85 
86  template<typename PointT> void
87  OutofcoreOctreeRamContainer<PointT>::insertRange (const PointT* start, const boost::uint64_t count)
88  {
89  container_.insert (container_.end (), start, start + count);
90  }
91 
92  ////////////////////////////////////////////////////////////////////////////////
93 
94  template<typename PointT> void
95  OutofcoreOctreeRamContainer<PointT>::insertRange (const PointT* const * start, const boost::uint64_t count)
96  {
97  AlignedPointTVector temp;
98  temp.resize (count);
99  for (boost::uint64_t i = 0; i < count; i++)
100  {
101  temp[i] = *start[i];
102  }
103  container_.insert (container_.end (), temp.begin (), temp.end ());
104  }
105 
106  ////////////////////////////////////////////////////////////////////////////////
107 
108  template<typename PointT> void
109  OutofcoreOctreeRamContainer<PointT>::readRange (const boost::uint64_t start, const boost::uint64_t count,
111  {
112  v.resize (count);
113  memcpy (v.data (), container_.data () + start, count * sizeof(PointT));
114  }
115 
116  ////////////////////////////////////////////////////////////////////////////////
117 
118  template<typename PointT> void
120  const boost::uint64_t count,
121  const double percent,
123  {
124  boost::uint64_t samplesize = static_cast<boost::uint64_t> (percent * static_cast<double> (count));
125 
126  boost::mutex::scoped_lock lock (rng_mutex_);
127 
128  boost::uniform_int < boost::uint64_t > buffdist (start, start + count);
129  boost::variate_generator<boost::mt19937&, boost::uniform_int<boost::uint64_t> > buffdie (rand_gen_, buffdist);
130 
131  for (boost::uint64_t i = 0; i < samplesize; i++)
132  {
133  boost::uint64_t buffstart = buffdie ();
134  v.push_back (container_[buffstart]);
135  }
136  }
137 
138  ////////////////////////////////////////////////////////////////////////////////
139 
140  }//namespace outofcore
141 }//namespace pcl
142 
143 #endif //PCL_OUTOFCORE_RAM_CONTAINER_IMPL_H_