random_sample.cpp
Go to the documentation of this file.
00001 /*
00002  * Software License Agreement (BSD License)
00003  *
00004  *  Point Cloud Library (PCL) - www.pointclouds.org
00005  *  Copyright (c) 2009, Willow Garage, Inc.
00006  *  Copyright (c) 2012-, Open Perception, 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 the copyright holder(s) 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: extract_indices.cpp 1385 2011-06-19 19:15:56Z rusu $
00038  *
00039  */
00040 
00041 #include <pcl/filters/impl/random_sample.hpp>
00042 
00044 void
00045 pcl::RandomSample<pcl::PCLPointCloud2>::applyFilter (PCLPointCloud2 &output)
00046 {
00047   unsigned N = input_->width * input_->height;
00048   // If sample size is 0 or if the sample size is greater then input cloud size
00049   //   then return entire copy of cloud
00050   if (sample_ >= N)
00051   {
00052     output = *input_;
00053   }
00054   else
00055   {
00056     // Resize output cloud to sample size
00057     output.data.resize (sample_ * input_->point_step);
00058 
00059     // Copy the common fields
00060     output.fields = input_->fields;
00061     output.is_bigendian = input_->is_bigendian;
00062     output.row_step = input_->row_step;
00063     output.point_step = input_->point_step;
00064     output.height = 1;
00065 
00066     // Set random seed so derived indices are the same each time the filter runs
00067     std::srand (seed_);
00068 
00069     unsigned top = N - sample_;
00070     unsigned i = 0;
00071     unsigned index = 0;
00072 
00073     // Algorithm A
00074     for (size_t n = sample_; n >= 2; n--)
00075     {
00076       float V = unifRand ();
00077       unsigned S = 0;
00078       float quot = float (top) / float (N);
00079       while (quot > V)
00080       {
00081         S++;
00082         top--;
00083         N--;
00084         quot = quot * float (top) / float (N);
00085       }
00086       index += S;
00087       memcpy (&output.data[i++ * output.point_step], &input_->data[index++ * output.point_step], output.point_step);
00088       N--;
00089     }
00090 
00091     index += N * static_cast<unsigned> (unifRand ());
00092     memcpy (&output.data[i++ * output.point_step], &input_->data[index++ * output.point_step], output.point_step);
00093 
00094     output.width = sample_;
00095     output.row_step = output.point_step * output.width;
00096   }
00097 }
00098 
00100 void
00101 pcl::RandomSample<pcl::PCLPointCloud2>::applyFilter (std::vector<int> &indices)
00102 {
00103   unsigned N = input_->width * input_->height;
00104   // If sample size is 0 or if the sample size is greater then input cloud size
00105   //   then return all indices
00106   if (sample_ >= N)
00107   {
00108     indices = *indices_;
00109   }
00110   else
00111   {
00112     // Resize output indices to sample size
00113     indices.resize (sample_);
00114 
00115     // Set random seed so derived indices are the same each time the filter runs
00116     std::srand (seed_);
00117 
00118     unsigned top = N - sample_;
00119     unsigned i = 0;
00120     unsigned index = 0;
00121 
00122     // Algorithm A
00123     for (size_t n = sample_; n >= 2; n--)
00124     {
00125       float V = unifRand ();
00126       unsigned S = 0;
00127       float quot = float (top) / float (N);
00128       while (quot > V)
00129       {
00130         S++;
00131         top--;
00132         N--;
00133         quot = quot * float (top) / float (N);
00134       }
00135       index += S;
00136       indices[i++] = (*indices_)[index++];
00137       N--;
00138     }
00139 
00140     index += N * static_cast<unsigned> (unifRand ());
00141     indices[i++] = (*indices_)[index++];
00142   }
00143 }
00144 
00145 #ifndef PCL_NO_PRECOMPILE
00146 #include <pcl/impl/instantiate.hpp>
00147 #include <pcl/point_types.h>
00148 
00149 PCL_INSTANTIATE(RandomSample, PCL_POINT_TYPES)
00150 
00151 #endif    // PCL_NO_PRECOMPILE
00152 


pcl
Author(s): Open Perception
autogenerated on Wed Aug 26 2015 15:31:54