octree_pointcloud_voxelcentroid.h
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) 2010-2011, Willow Garage, Inc.
00006  *
00007  *  All rights reserved.
00008  *
00009  *  Redistribution and use in source and binary forms, with or without
00010  *  modification, are permitted provided that the following conditions
00011  *  are met:
00012  *
00013  *   * Redistributions of source code must retain the above copyright
00014  *     notice, this list of conditions and the following disclaimer.
00015  *   * Redistributions in binary form must reproduce the above
00016  *     copyright notice, this list of conditions and the following
00017  *     disclaimer in the documentation and/or other materials provided
00018  *     with the distribution.
00019  *   * Neither the name of Willow Garage, Inc. nor the names of its
00020  *     contributors may be used to endorse or promote products derived
00021  *     from this software without specific prior written permission.
00022  *
00023  *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
00024  *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
00025  *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
00026  *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
00027  *  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
00028  *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
00029  *  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
00030  *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
00031  *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
00032  *  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
00033  *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
00034  *  POSSIBILITY OF SUCH DAMAGE.
00035  *
00036  * $Id: octree_pointcloud_voxelcentroid.h 6119 2012-07-03 18:50:04Z aichim $
00037  */
00038 
00039 #ifndef OCTREE_VOXELCENTROID_H
00040 #define OCTREE_VOXELCENTROID_H
00041 
00042 #include "octree_pointcloud.h"
00043 
00044 #include "octree_base.h"
00045 #include "octree2buf_base.h"
00046 
00047 namespace pcl
00048 {
00049   namespace octree
00050   {
00051 
00053 
00061 
00062     template<typename PointT, typename LeafT = OctreeContainerDataTVector<int> , typename BranchT = OctreeContainerEmpty<int> >
00063       class OctreePointCloudVoxelCentroid : public OctreePointCloud<PointT, LeafT, BranchT>
00064       {
00065 
00066       public:
00067         // public typedefs for single/double buffering
00068 
00069         // Eigen aligned allocator
00070         typedef typename OctreePointCloud<PointT, LeafT, BranchT>::AlignedPointTVector AlignedPointTVector;
00071 
00075         OctreePointCloudVoxelCentroid (const double resolution_arg) :
00076           OctreePointCloud<PointT, LeafT, BranchT> (resolution_arg)
00077         {
00078         }
00079 
00081         virtual
00082         ~OctreePointCloudVoxelCentroid ()
00083         {
00084         }
00085 
00090         unsigned int
00091         getVoxelCentroids (AlignedPointTVector &voxelCentroidList_arg)
00092         {
00093 
00094           size_t i;
00095           unsigned int pointCounter;
00096           OctreeKey keyC, keyP;
00097           PointT meanPoint;
00098           PointT idxPoint;
00099 
00100           std::vector<int> indicesVector;
00101 
00102           voxelCentroidList_arg.clear();
00103           voxelCentroidList_arg.reserve(this->leafCount_);
00104 
00105           // serialize leafs - this returns a list of point indices. Points indices from the same voxel are locates next to each other within this vector.
00106           this->serializeLeafs (indicesVector);
00107 
00108           // initializing
00109           keyP.x = keyP.y = keyP.z = std::numeric_limits<unsigned int>::max ();
00110           meanPoint.x = meanPoint.y = meanPoint.z = 0.0;
00111           pointCounter = 0;
00112 
00113           // iterate over all point indices
00114           for (i = 0; i < indicesVector.size (); i++)
00115           {
00116             idxPoint = this->input_->points[indicesVector[i]];
00117 
00118             // get octree key for point (key specifies octree voxel)
00119             this->genOctreeKeyforPoint (idxPoint, keyC);
00120 
00121             if (keyC == keyP)
00122             {
00123               // key addresses same voxel - add point
00124               meanPoint.x += idxPoint.x;
00125               meanPoint.y += idxPoint.y;
00126               meanPoint.z += idxPoint.z;
00127 
00128               pointCounter++;
00129             }
00130             else
00131             {
00132               // voxel key did change - calculate centroid and push it to result vector
00133               if (pointCounter > 0)
00134               {
00135                 meanPoint.x /= static_cast<float> (pointCounter);
00136                 meanPoint.y /= static_cast<float> (pointCounter);
00137                 meanPoint.z /= static_cast<float> (pointCounter);
00138 
00139                 voxelCentroidList_arg.push_back (meanPoint);
00140               }
00141 
00142               // reset centroid to current input point
00143               meanPoint.x = idxPoint.x;
00144               meanPoint.y = idxPoint.y;
00145               meanPoint.z = idxPoint.z;
00146               pointCounter = 1;
00147 
00148               keyP = keyC;
00149             }
00150           }
00151 
00152           // push last centroid to result vector if necessary
00153           if (pointCounter > 0)
00154           {
00155             meanPoint.x /= static_cast<float> (pointCounter);
00156             meanPoint.y /= static_cast<float> (pointCounter);
00157             meanPoint.z /= static_cast<float> (pointCounter);
00158 
00159             voxelCentroidList_arg.push_back (meanPoint);
00160           }
00161 
00162           // return size of centroid vector
00163           return (static_cast<unsigned int> (voxelCentroidList_arg.size ()));
00164         }
00165 
00171         bool
00172         getVoxelCentroidAtPoint (const PointT& point_arg, PointT& voxelCentroid_arg)
00173         {
00174 
00175           size_t i;
00176           unsigned int pointCounter;
00177           std::vector<int> indicesVector;
00178           PointT meanPoint;
00179           PointT idxPoint;
00180 
00181           bool bResult;
00182 
00183           // get all point indixes from voxel at point point_arg
00184           bResult = this->voxelSearch (point_arg, indicesVector);
00185 
00186           if (bResult)
00187           {
00188             meanPoint.x = meanPoint.y = meanPoint.z = 0.0;
00189             pointCounter = 0;
00190 
00191             // iterate over all point indices
00192             for (i = 0; i < indicesVector.size (); i++)
00193             {
00194               idxPoint = this->input_->points[indicesVector[i]];
00195 
00196               meanPoint.x += idxPoint.x;
00197               meanPoint.y += idxPoint.y;
00198               meanPoint.z += idxPoint.z;
00199 
00200               pointCounter++;
00201             }
00202 
00203             // calculate centroid
00204             voxelCentroid_arg.x = meanPoint.x / static_cast<float> (pointCounter);
00205             voxelCentroid_arg.y = meanPoint.y / static_cast<float> (pointCounter);
00206             voxelCentroid_arg.z = meanPoint.z / static_cast<float> (pointCounter);
00207           }
00208 
00209           return (bResult);
00210         }
00211 
00217         inline bool
00218         getVoxelCentroidAtPoint (const int& pointIdx_arg, PointT& voxelCentroid_arg)
00219         {
00220 
00221           // retrieve point from input cloud
00222           const PointT& point = this->input_->points[pointIdx_arg];
00223 
00224           // get centroid at point
00225           return this->getVoxelCentroidAtPoint (point, voxelCentroid_arg);
00226 
00227         }
00228 
00229       };
00230 
00231   }
00232 
00233 }
00234 
00235 #define PCL_INSTANTIATE_OctreePointCloudVoxelCentroid(T) template class PCL_EXPORTS pcl::octree::OctreePointCloudVoxelCentroid<T>;
00236 
00237 #endif
00238 


pcl
Author(s): Open Perception
autogenerated on Mon Oct 6 2014 03:15:58