Point Cloud Library (PCL)
1.7.0
|
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 * 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$ 00038 */ 00039 00040 #ifndef PCL_OCTREE_VOXELCENTROID_H 00041 #define PCL_OCTREE_VOXELCENTROID_H 00042 00043 #include "octree_pointcloud.h" 00044 00045 #include <pcl/common/point_operators.h> 00046 #include <pcl/point_types.h> 00047 #include <pcl/register_point_struct.h> 00048 00049 namespace pcl 00050 { 00051 namespace octree 00052 { 00053 /** \brief @b Octree pointcloud voxel centroid leaf node class 00054 * \note This class implements a leaf node that calculates the mean centroid of all points added this octree container. 00055 * \author Julius Kammerl (julius@kammerl.de) 00056 */ 00057 template<typename PointT> 00058 class OctreePointCloudVoxelCentroidContainer : public OctreeContainerBase 00059 { 00060 public: 00061 /** \brief Class initialization. */ 00062 OctreePointCloudVoxelCentroidContainer () 00063 { 00064 this->reset(); 00065 } 00066 00067 /** \brief Empty class deconstructor. */ 00068 virtual ~OctreePointCloudVoxelCentroidContainer () 00069 { 00070 } 00071 00072 /** \brief deep copy function */ 00073 virtual OctreePointCloudVoxelCentroidContainer * 00074 deepCopy () const 00075 { 00076 return (new OctreePointCloudVoxelCentroidContainer (*this)); 00077 } 00078 00079 /** \brief Equal comparison operator - set to false 00080 * \param[in] OctreePointCloudVoxelCentroidContainer to compare with 00081 */ 00082 virtual bool operator==(const OctreeContainerBase&) const 00083 { 00084 return ( false ); 00085 } 00086 00087 /** \brief Add new point to voxel. 00088 * \param[in] new_point the new point to add 00089 */ 00090 void 00091 addPoint (const PointT& new_point) 00092 { 00093 using namespace pcl::common; 00094 00095 ++point_counter_; 00096 00097 point_sum_ += new_point; 00098 } 00099 00100 /** \brief Calculate centroid of voxel. 00101 * \param[out] centroid_arg the resultant centroid of the voxel 00102 */ 00103 void 00104 getCentroid (PointT& centroid_arg) const 00105 { 00106 using namespace pcl::common; 00107 00108 if (point_counter_) 00109 { 00110 centroid_arg = point_sum_; 00111 centroid_arg /= static_cast<float> (point_counter_); 00112 } 00113 else 00114 { 00115 centroid_arg *= 0.0f; 00116 } 00117 } 00118 00119 /** \brief Reset leaf container. */ 00120 virtual void 00121 reset () 00122 { 00123 using namespace pcl::common; 00124 00125 point_counter_ = 0; 00126 point_sum_ *= 0.0f; 00127 } 00128 00129 private: 00130 unsigned int point_counter_; 00131 PointT point_sum_; 00132 }; 00133 00134 /** \brief @b Octree pointcloud voxel centroid class 00135 * \note This class generate an octrees from a point cloud (zero-copy). It provides a vector of centroids for all occupied voxels. 00136 * \note The octree pointcloud is initialized with its voxel resolution. Its bounding box is automatically adjusted or can be predefined. 00137 * \note 00138 * \note typename: PointT: type of point used in pointcloud 00139 * 00140 * \ingroup octree 00141 * \author Julius Kammerl (julius@kammerl.de) 00142 */ 00143 template<typename PointT, 00144 typename LeafContainerT = OctreePointCloudVoxelCentroidContainer<PointT> , 00145 typename BranchContainerT = OctreeContainerEmpty > 00146 class OctreePointCloudVoxelCentroid : public OctreePointCloud<PointT, LeafContainerT, BranchContainerT> 00147 { 00148 public: 00149 typedef boost::shared_ptr<OctreePointCloudVoxelCentroid<PointT, LeafContainerT> > Ptr; 00150 typedef boost::shared_ptr<const OctreePointCloudVoxelCentroid<PointT, LeafContainerT> > ConstPtr; 00151 00152 typedef OctreePointCloud<PointT, LeafContainerT, BranchContainerT> OctreeT; 00153 typedef typename OctreeT::LeafNode LeafNode; 00154 typedef typename OctreeT::BranchNode BranchNode; 00155 00156 /** \brief OctreePointCloudVoxelCentroids class constructor. 00157 * \param[in] resolution_arg octree resolution at lowest octree level 00158 */ 00159 OctreePointCloudVoxelCentroid (const double resolution_arg) : 00160 OctreePointCloud<PointT, LeafContainerT, BranchContainerT> (resolution_arg) 00161 { 00162 } 00163 00164 /** \brief Empty class deconstructor. */ 00165 virtual 00166 ~OctreePointCloudVoxelCentroid () 00167 { 00168 } 00169 00170 /** \brief Add DataT object to leaf node at octree key. 00171 * \param[in] key_arg octree key addressing a leaf node. 00172 * \param[in] data_arg DataT object to be added. 00173 */ 00174 virtual void 00175 addPointIdx (const int pointIdx_arg) 00176 { 00177 OctreeKey key; 00178 00179 assert (pointIdx_arg < static_cast<int> (this->input_->points.size ())); 00180 00181 const PointT& point = this->input_->points[pointIdx_arg]; 00182 00183 // make sure bounding box is big enough 00184 this->adoptBoundingBoxToPoint (point); 00185 00186 // generate key 00187 this->genOctreeKeyforPoint (point, key); 00188 00189 // add point to octree at key 00190 LeafContainerT* container = this->createLeaf(key); 00191 container->addPoint (point); 00192 00193 } 00194 00195 /** \brief Get centroid for a single voxel addressed by a PointT point. 00196 * \param[in] point_arg point addressing a voxel in octree 00197 * \param[out] voxel_centroid_arg centroid is written to this PointT reference 00198 * \return "true" if voxel is found; "false" otherwise 00199 */ 00200 bool 00201 getVoxelCentroidAtPoint (const PointT& point_arg, PointT& voxel_centroid_arg) const; 00202 00203 /** \brief Get centroid for a single voxel addressed by a PointT point from input cloud. 00204 * \param[in] point_idx_arg point index from input cloud addressing a voxel in octree 00205 * \param[out] voxel_centroid_arg centroid is written to this PointT reference 00206 * \return "true" if voxel is found; "false" otherwise 00207 */ 00208 inline bool 00209 getVoxelCentroidAtPoint (const int& point_idx_arg, PointT& voxel_centroid_arg) const 00210 { 00211 // get centroid at point 00212 return (this->getVoxelCentroidAtPoint (this->input_->points[point_idx_arg], voxel_centroid_arg)); 00213 } 00214 00215 /** \brief Get PointT vector of centroids for all occupied voxels. 00216 * \param[out] voxel_centroid_list_arg results are written to this vector of PointT elements 00217 * \return number of occupied voxels 00218 */ 00219 size_t 00220 getVoxelCentroids (typename OctreePointCloud<PointT, LeafContainerT, BranchContainerT>::AlignedPointTVector &voxel_centroid_list_arg) const; 00221 00222 /** \brief Recursively explore the octree and output a PointT vector of centroids for all occupied voxels. 00223 * \param[in] branch_arg: current branch node 00224 * \param[in] key_arg: current key 00225 * \param[out] voxel_centroid_list_arg results are written to this vector of PointT elements 00226 */ 00227 void 00228 getVoxelCentroidsRecursive (const BranchNode* branch_arg, 00229 OctreeKey& key_arg, 00230 typename OctreePointCloud<PointT, LeafContainerT, BranchContainerT>::AlignedPointTVector &voxel_centroid_list_arg) const; 00231 00232 }; 00233 } 00234 } 00235 00236 #include <pcl/octree/impl/octree_pointcloud_voxelcentroid.hpp> 00237 00238 #endif 00239