octree_ram_container.hpp

Go to the documentation of this file.

/*
 * Software License Agreement (BSD License)
 *
 *  Point Cloud Library (PCL) - www.pointclouds.org
 *  Copyright (c) 2010-2012, Willow Garage, Inc.
 *  Copyright (c) 2012, Urban Robotics, Inc.
 *
 *  All rights reserved.
 *
 *  Redistribution and use in source and binary forms, with or without
 *  modification, are permitted provided that the following conditions
 *  are met:
 *
 *   * Redistributions of source code must retain the above copyright
 *     notice, this list of conditions and the following disclaimer.
 *   * Redistributions in binary form must reproduce the above
 *     copyright notice, this list of conditions and the following
 *     disclaimer in the documentation and/or other materials provided
 *     with the distribution.
 *   * Neither the name of Willow Garage, Inc. nor the names of its
 *     contributors may be used to endorse or promote products derived
 *     from this software without specific prior written permission.
 *
 *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
 *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
 *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
 *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
 *  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
 *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
 *  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
 *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
 *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
 *  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
 *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
 *  POSSIBILITY OF SUCH DAMAGE.
 *
 *  $Id: octree_ram_container.hpp 6927 2012-08-23 02:34:54Z stfox88 $
 */

#ifndef PCL_OUTOFCORE_RAM_CONTAINER_IMPL_H_
#define PCL_OUTOFCORE_RAM_CONTAINER_IMPL_H_

// C++
#include <sstream>

// PCL (Urban Robotics)
#include <pcl/outofcore/octree_ram_container.h>

namespace pcl
{
  namespace outofcore
  {

    template<typename PointT>
    boost::mutex OutofcoreOctreeRamContainer<PointT>::rng_mutex_;

    template<typename PointT> 
    boost::mt19937 OutofcoreOctreeRamContainer<PointT>::rand_gen_ (static_cast<unsigned int>(std::time( NULL)));

    template<typename PointT> void
    OutofcoreOctreeRamContainer<PointT>::convertToXYZ (const boost::filesystem::path& path)
    {
      if (!container_.empty ())
      {
        FILE* fxyz = fopen (path.string ().c_str (), "w");

        boost::uint64_t num = size ();
        for (boost::uint64_t i = 0; i < num; i++)
        {
          const PointT& p = container_[i];

          std::stringstream ss;
          ss << std::fixed;
          ss.precision (16);
          ss << p.x << "\t" << p.y << "\t" << p.z << "\n";

          fwrite (ss.str ().c_str (), 1, ss.str ().size (), fxyz);
        }

        assert ( fclose (fxyz) == 0 );
      }
    }


    template<typename PointT> void
    OutofcoreOctreeRamContainer<PointT>::insertRange (const PointT* start, const boost::uint64_t count)
    {
      container_.insert (container_.end (), start, start + count);
    }


    template<typename PointT> void
    OutofcoreOctreeRamContainer<PointT>::insertRange (const PointT* const * start, const boost::uint64_t count)
    {
      AlignedPointTVector temp;
      temp.resize (count);
      for (boost::uint64_t i = 0; i < count; i++)
      {
        temp[i] = *start[i];
      }
      container_.insert (container_.end (), temp.begin (), temp.end ());
    }


    template<typename PointT> void
    OutofcoreOctreeRamContainer<PointT>::readRange (const boost::uint64_t start, const boost::uint64_t count,
                                             AlignedPointTVector& v)
    {
      v.resize (count);
      memcpy (v.data (), container_.data () + start, count * sizeof(PointT));
    }


    template<typename PointT> void
    OutofcoreOctreeRamContainer<PointT>::readRangeSubSample (const boost::uint64_t start, 
                                                      const boost::uint64_t count,
                                                      const double percent, 
                                                      AlignedPointTVector& v)
    {
      boost::uint64_t samplesize = static_cast<boost::uint64_t> (percent * static_cast<double> (count));

      boost::mutex::scoped_lock lock (rng_mutex_);

      boost::uniform_int < boost::uint64_t > buffdist (start, start + count);
      boost::variate_generator<boost::mt19937&, boost::uniform_int<boost::uint64_t> > buffdie (rand_gen_, buffdist);

      for (boost::uint64_t i = 0; i < samplesize; i++)
      {
        boost::uint64_t buffstart = buffdie ();
        v.push_back (container_[buffstart]);
      }
    }


  }//namespace outofcore
}//namespace pcl

#endif //PCL_OUTOFCORE_RAM_CONTAINER_IMPL_H_