Point Cloud Library (PCL)  1.3.1
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 3017 2011-11-01 03:24:04Z rusu $
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 = OctreeLeafDataTVector<int> , typename OctreeT = OctreeBase<int, LeafT> >
00063       class OctreePointCloudVoxelCentroid : public OctreePointCloud<PointT, LeafT, OctreeT>
00064       {
00065 
00066       public:
00067         // public typedefs for single/double buffering
00068         typedef OctreePointCloudVoxelCentroid<PointT, LeafT, OctreeBase<int, LeafT> > SingleBuffer;
00069         typedef OctreePointCloudVoxelCentroid<PointT, LeafT, Octree2BufBase<int, LeafT> > DoubleBuffer;
00070 
00074         OctreePointCloudVoxelCentroid (const double resolution_arg) :
00075           OctreePointCloud<PointT, LeafT, OctreeT> (resolution_arg)
00076         {
00077         }
00078 
00080         virtual
00081         ~OctreePointCloudVoxelCentroid ()
00082         {
00083         }
00084 
00089         unsigned int
00090         getVoxelCentroids (std::vector<PointT, Eigen::aligned_allocator<PointT> > &voxelCentroidList_arg)
00091         {
00092 
00093           size_t i;
00094           unsigned int pointCounter;
00095           typename OctreePointCloud<PointT, LeafT, OctreeT>::OctreeKey keyC, keyP;
00096           PointT meanPoint;
00097           PointT idxPoint;
00098 
00099           std::vector<int> indicesVector;
00100 
00101           voxelCentroidList_arg.clear();
00102           voxelCentroidList_arg.reserve(this->leafCount_);
00103 
00104           // serialize leafs - this returns a list of point indices. Points indices from the same voxel are locates next to each other within this vector.
00105           this->serializeLeafs (indicesVector);
00106 
00107           // initializing
00108           keyP.x = keyP.y = keyP.z = std::numeric_limits<unsigned int>::max ();
00109           meanPoint.x = meanPoint.y = meanPoint.z = 0.0;
00110           pointCounter = 0;
00111 
00112           // iterate over all point indices
00113           for (i = 0; i < indicesVector.size (); i++)
00114           {
00115             idxPoint = this->input_->points[indicesVector[i]];
00116 
00117             // get octree key for point (key specifies octree voxel)
00118             this->genOctreeKeyforPoint (idxPoint, keyC);
00119 
00120             if (keyC == keyP)
00121             {
00122               // key addresses same voxel - add point
00123               meanPoint.x += idxPoint.x;
00124               meanPoint.y += idxPoint.y;
00125               meanPoint.z += idxPoint.z;
00126 
00127               pointCounter++;
00128             }
00129             else
00130             {
00131               // voxel key did change - calculate centroid and push it to result vector
00132               if (pointCounter > 0)
00133               {
00134                 meanPoint.x /= (float)pointCounter;
00135                 meanPoint.y /= (float)pointCounter;
00136                 meanPoint.z /= (float)pointCounter;
00137 
00138                 voxelCentroidList_arg.push_back (meanPoint);
00139               }
00140 
00141               // reset centroid to current input point
00142               meanPoint.x = idxPoint.x;
00143               meanPoint.y = idxPoint.y;
00144               meanPoint.z = idxPoint.z;
00145               pointCounter = 1;
00146 
00147               keyP = keyC;
00148             }
00149           }
00150 
00151           // push last centroid to result vector if necessary
00152           if (pointCounter > 0)
00153           {
00154             meanPoint.x /= (float)pointCounter;
00155             meanPoint.y /= (float)pointCounter;
00156             meanPoint.z /= (float)pointCounter;
00157 
00158             voxelCentroidList_arg.push_back (meanPoint);
00159           }
00160 
00161           // return size of centroid vector
00162           return voxelCentroidList_arg.size ();
00163         }
00164 
00170         bool
00171         getVoxelCentroidAtPoint (const PointT& point_arg, PointT& voxelCentroid_arg)
00172         {
00173 
00174           size_t i;
00175           unsigned int pointCounter;
00176           std::vector<int> indicesVector;
00177           PointT meanPoint;
00178           PointT idxPoint;
00179 
00180           bool bResult;
00181 
00182           // get all point indixes from voxel at point point_arg
00183           bResult = this->voxelSearch (point_arg, indicesVector);
00184 
00185           if (bResult)
00186           {
00187             meanPoint.x = meanPoint.y = meanPoint.z = 0.0;
00188             pointCounter = 0;
00189 
00190             // iterate over all point indices
00191             for (i = 0; i < indicesVector.size (); i++)
00192             {
00193               idxPoint = this->input_->points[indicesVector[i]];
00194 
00195               meanPoint.x += idxPoint.x;
00196               meanPoint.y += idxPoint.y;
00197               meanPoint.z += idxPoint.z;
00198 
00199               pointCounter++;
00200             }
00201 
00202             // calculate centroid
00203             voxelCentroid_arg.x = meanPoint.x / (float)pointCounter;
00204             voxelCentroid_arg.y = meanPoint.y / (float)pointCounter;
00205             voxelCentroid_arg.z = meanPoint.z / (float)pointCounter;
00206           }
00207 
00208           return bResult;
00209         }
00210 
00216         inline bool
00217         getVoxelCentroidAtPoint (const int& pointIdx_arg, PointT& voxelCentroid_arg)
00218         {
00219 
00220           // retrieve point from input cloud
00221           const PointT& point = this->input_->points[pointIdx_arg];
00222 
00223           // get centroid at point
00224           return this->getVoxelCentroidAtPoint (point, voxelCentroid_arg);
00225 
00226         }
00227 
00228       };
00229 
00230   }
00231 
00232 }
00233 
00234 #define PCL_INSTANTIATE_OctreePointCloudVoxelCentroid(T) template class PCL_EXPORTS pcl::octree::OctreePointCloudVoxelCentroid<T>;
00235 
00236 #endif
00237 
 All Classes Namespaces Files Functions Variables Typedefs Enumerations Enumerator Friends Defines