1 /* 2 * Software License Agreement (BSD License) 3 * 4 * Point Cloud Library (PCL) - www.pointclouds.org 5 * Copyright (c) 2010-2011, Willow Garage, Inc. 6 * Copyright (c) 2012-, Open Perception, Inc. 7 * 8 * All rights reserved. 9 * 10 * Redistribution and use in source and binary forms, with or without 11 * modification, are permitted provided that the following conditions 12 * are met: 13 * 14 * * Redistributions of source code must retain the above copyright 15 * notice, this list of conditions and the following disclaimer. 16 * * Redistributions in binary form must reproduce the above 17 * copyright notice, this list of conditions and the following 18 * disclaimer in the documentation and/or other materials provided 19 * with the distribution. 20 * * Neither the name of the copyright holder(s) nor the names of its 21 * contributors may be used to endorse or promote products derived 22 * from this software without specific prior written permission. 23 * 24 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 25 * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 26 * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 27 * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 28 * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, 29 * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, 30 * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 31 * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 32 * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT 33 * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN 34 * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 35 * POSSIBILITY OF SUCH DAMAGE. 36 * 37 * $Id$ 38 */ 39 40 #pragma once 41 42 #include <pcl/octree/octree_pointcloud.h> 43 44 namespace pcl { 45 namespace octree { 46 /** \brief @b Octree pointcloud voxel centroid leaf node class 47 * \note This class implements a leaf node that calculates the mean centroid of all 48 * points added this octree container. 49 * \author Julius Kammerl (julius@kammerl.de) 50 */ 51 template <typename PointT> 52 class OctreePointCloudVoxelCentroidContainer : public OctreeContainerBase { 53 public: 54 /** \brief Class initialization. */ OctreePointCloudVoxelCentroidContainer()55 OctreePointCloudVoxelCentroidContainer() { this->reset(); } 56 57 /** \brief Empty class deconstructor. */ ~OctreePointCloudVoxelCentroidContainer()58 ~OctreePointCloudVoxelCentroidContainer() {} 59 60 /** \brief deep copy function */ 61 virtual OctreePointCloudVoxelCentroidContainer* deepCopy()62 deepCopy() const 63 { 64 return (new OctreePointCloudVoxelCentroidContainer(*this)); 65 } 66 67 /** \brief Equal comparison operator - set to false 68 */ 69 // param[in] OctreePointCloudVoxelCentroidContainer to compare with 70 bool 71 operator==(const OctreeContainerBase&) const override 72 { 73 return (false); 74 } 75 76 /** \brief Add new point to voxel. 77 * \param[in] new_point the new point to add 78 */ 79 void addPoint(const PointT & new_point)80 addPoint(const PointT& new_point) 81 { 82 using namespace pcl::common; 83 84 ++point_counter_; 85 86 point_sum_ += new_point; 87 } 88 89 /** \brief Calculate centroid of voxel. 90 * \param[out] centroid_arg the resultant centroid of the voxel 91 */ 92 void getCentroid(PointT & centroid_arg)93 getCentroid(PointT& centroid_arg) const 94 { 95 using namespace pcl::common; 96 97 if (point_counter_) { 98 centroid_arg = point_sum_; 99 centroid_arg /= static_cast<float>(point_counter_); 100 } 101 else { 102 centroid_arg *= 0.0f; 103 } 104 } 105 106 /** \brief Reset leaf container. */ 107 void reset()108 reset() override 109 { 110 using namespace pcl::common; 111 112 point_counter_ = 0; 113 point_sum_ *= 0.0f; 114 } 115 116 private: 117 uindex_t point_counter_; 118 PointT point_sum_; 119 }; 120 121 /** \brief @b Octree pointcloud voxel centroid class 122 * \note This class generate an octrees from a point cloud (zero-copy). It provides a 123 * vector of centroids for all occupied voxels. 124 * \note The octree pointcloud is initialized with its voxel resolution. Its bounding 125 * box is automatically adjusted or can be predefined. 126 * \tparam PointT type of point used in pointcloud 127 * \ingroup octree 128 * \author Julius Kammerl (julius@kammerl.de) 129 */ 130 template <typename PointT, 131 typename LeafContainerT = OctreePointCloudVoxelCentroidContainer<PointT>, 132 typename BranchContainerT = OctreeContainerEmpty> 133 class OctreePointCloudVoxelCentroid 134 : public OctreePointCloud<PointT, LeafContainerT, BranchContainerT> { 135 public: 136 using Ptr = shared_ptr<OctreePointCloudVoxelCentroid<PointT, LeafContainerT>>; 137 using ConstPtr = 138 shared_ptr<const OctreePointCloudVoxelCentroid<PointT, LeafContainerT>>; 139 140 using OctreeT = OctreePointCloud<PointT, LeafContainerT, BranchContainerT>; 141 using LeafNode = typename OctreeT::LeafNode; 142 using BranchNode = typename OctreeT::BranchNode; 143 144 /** \brief OctreePointCloudVoxelCentroids class constructor. 145 * \param[in] resolution_arg octree resolution at lowest octree level 146 */ OctreePointCloudVoxelCentroid(const double resolution_arg)147 OctreePointCloudVoxelCentroid(const double resolution_arg) 148 : OctreePointCloud<PointT, LeafContainerT, BranchContainerT>(resolution_arg) 149 {} 150 151 /** \brief Empty class deconstructor. */ 152 ~OctreePointCloudVoxelCentroid()153 ~OctreePointCloudVoxelCentroid() {} 154 155 /** \brief Add DataT object to leaf node at octree key. 156 * \param pointIdx_arg 157 */ 158 void addPointIdx(const uindex_t pointIdx_arg)159 addPointIdx(const uindex_t pointIdx_arg) override 160 { 161 OctreeKey key; 162 163 assert(pointIdx_arg < this->input_->size()); 164 165 const PointT& point = (*this->input_)[pointIdx_arg]; 166 167 // make sure bounding box is big enough 168 this->adoptBoundingBoxToPoint(point); 169 170 // generate key 171 this->genOctreeKeyforPoint(point, key); 172 173 // add point to octree at key 174 LeafContainerT* container = this->createLeaf(key); 175 container->addPoint(point); 176 } 177 178 /** \brief Get centroid for a single voxel addressed by a PointT point. 179 * \param[in] point_arg point addressing a voxel in octree 180 * \param[out] voxel_centroid_arg centroid is written to this PointT reference 181 * \return "true" if voxel is found; "false" otherwise 182 */ 183 bool 184 getVoxelCentroidAtPoint(const PointT& point_arg, PointT& voxel_centroid_arg) const; 185 186 /** \brief Get centroid for a single voxel addressed by a PointT point from input 187 * cloud. 188 * \param[in] point_idx_arg point index from input cloud addressing a voxel in octree 189 * \param[out] voxel_centroid_arg centroid is written to this PointT reference 190 * \return "true" if voxel is found; "false" otherwise 191 */ 192 inline bool getVoxelCentroidAtPoint(const index_t & point_idx_arg,PointT & voxel_centroid_arg)193 getVoxelCentroidAtPoint(const index_t& point_idx_arg, 194 PointT& voxel_centroid_arg) const 195 { 196 // get centroid at point 197 return (this->getVoxelCentroidAtPoint((*this->input_)[point_idx_arg], 198 voxel_centroid_arg)); 199 } 200 201 /** \brief Get PointT vector of centroids for all occupied voxels. 202 * \param[out] voxel_centroid_list_arg results are written to this vector of PointT 203 * elements 204 * \return number of occupied voxels 205 */ 206 uindex_t 207 getVoxelCentroids( 208 typename OctreePointCloud<PointT, LeafContainerT, BranchContainerT>:: 209 AlignedPointTVector& voxel_centroid_list_arg) const; 210 211 /** \brief Recursively explore the octree and output a PointT vector of centroids for 212 * all occupied voxels. 213 * \param[in] branch_arg: current branch node 214 * \param[in] key_arg: current key 215 * \param[out] voxel_centroid_list_arg results are written to this vector of PointT 216 * elements 217 */ 218 void 219 getVoxelCentroidsRecursive( 220 const BranchNode* branch_arg, 221 OctreeKey& key_arg, 222 typename OctreePointCloud<PointT, LeafContainerT, BranchContainerT>:: 223 AlignedPointTVector& voxel_centroid_list_arg) const; 224 }; 225 } // namespace octree 226 } // namespace pcl 227 228 // Note: Don't precompile this octree type to speed up compilation. It's probably rarely 229 // used. 230 #include <pcl/octree/impl/octree_pointcloud_voxelcentroid.hpp> 231