Point Cloud Library (PCL)  1.7.1
octree_pointcloud_voxelcentroid.h
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 #ifndef PCL_OCTREE_VOXELCENTROID_H
41 #define PCL_OCTREE_VOXELCENTROID_H
42 
43 #include "octree_pointcloud.h"
44 
45 #include <pcl/common/point_operators.h>
46 #include <pcl/point_types.h>
47 #include <pcl/register_point_struct.h>
48 
49 namespace pcl
50 {
51  namespace octree
52  {
53  /** \brief @b Octree pointcloud voxel centroid leaf node class
54  * \note This class implements a leaf node that calculates the mean centroid of all points added this octree container.
55  * \author Julius Kammerl (julius@kammerl.de)
56  */
57  template<typename PointT>
59  {
60  public:
61  /** \brief Class initialization. */
63  {
64  this->reset();
65  }
66 
67  /** \brief Empty class deconstructor. */
69  {
70  }
71 
72  /** \brief deep copy function */
74  deepCopy () const
75  {
76  return (new OctreePointCloudVoxelCentroidContainer (*this));
77  }
78 
79  /** \brief Equal comparison operator - set to false
80  * \param[in] OctreePointCloudVoxelCentroidContainer to compare with
81  */
82  virtual bool operator==(const OctreeContainerBase&) const
83  {
84  return ( false );
85  }
86 
87  /** \brief Add new point to voxel.
88  * \param[in] new_point the new point to add
89  */
90  void
91  addPoint (const PointT& new_point)
92  {
93  using namespace pcl::common;
94 
95  ++point_counter_;
96 
97  point_sum_ += new_point;
98  }
99 
100  /** \brief Calculate centroid of voxel.
101  * \param[out] centroid_arg the resultant centroid of the voxel
102  */
103  void
104  getCentroid (PointT& centroid_arg) const
105  {
106  using namespace pcl::common;
107 
108  if (point_counter_)
109  {
110  centroid_arg = point_sum_;
111  centroid_arg /= static_cast<float> (point_counter_);
112  }
113  else
114  {
115  centroid_arg *= 0.0f;
116  }
117  }
118 
119  /** \brief Reset leaf container. */
120  virtual void
121  reset ()
122  {
123  using namespace pcl::common;
124 
125  point_counter_ = 0;
126  point_sum_ *= 0.0f;
127  }
128 
129  private:
130  unsigned int point_counter_;
131  PointT point_sum_;
132  };
133 
134  /** \brief @b Octree pointcloud voxel centroid class
135  * \note This class generate an octrees from a point cloud (zero-copy). It provides a vector of centroids for all occupied voxels.
136  * \note The octree pointcloud is initialized with its voxel resolution. Its bounding box is automatically adjusted or can be predefined.
137  * \note
138  * \note typename: PointT: type of point used in pointcloud
139  *
140  * \ingroup octree
141  * \author Julius Kammerl (julius@kammerl.de)
142  */
143  template<typename PointT,
144  typename LeafContainerT = OctreePointCloudVoxelCentroidContainer<PointT> ,
145  typename BranchContainerT = OctreeContainerEmpty >
146  class OctreePointCloudVoxelCentroid : public OctreePointCloud<PointT, LeafContainerT, BranchContainerT>
147  {
148  public:
149  typedef boost::shared_ptr<OctreePointCloudVoxelCentroid<PointT, LeafContainerT> > Ptr;
150  typedef boost::shared_ptr<const OctreePointCloudVoxelCentroid<PointT, LeafContainerT> > ConstPtr;
151 
153  typedef typename OctreeT::LeafNode LeafNode;
154  typedef typename OctreeT::BranchNode BranchNode;
155 
156  /** \brief OctreePointCloudVoxelCentroids class constructor.
157  * \param[in] resolution_arg octree resolution at lowest octree level
158  */
159  OctreePointCloudVoxelCentroid (const double resolution_arg) :
160  OctreePointCloud<PointT, LeafContainerT, BranchContainerT> (resolution_arg)
161  {
162  }
163 
164  /** \brief Empty class deconstructor. */
165  virtual
167  {
168  }
169 
170  /** \brief Add DataT object to leaf node at octree key.
171  * \param[in] key_arg octree key addressing a leaf node.
172  * \param[in] data_arg DataT object to be added.
173  */
174  virtual void
175  addPointIdx (const int pointIdx_arg)
176  {
177  OctreeKey key;
178 
179  assert (pointIdx_arg < static_cast<int> (this->input_->points.size ()));
180 
181  const PointT& point = this->input_->points[pointIdx_arg];
182 
183  // make sure bounding box is big enough
184  this->adoptBoundingBoxToPoint (point);
185 
186  // generate key
187  this->genOctreeKeyforPoint (point, key);
188 
189  // add point to octree at key
190  LeafContainerT* container = this->createLeaf(key);
191  container->addPoint (point);
192 
193  }
194 
195  /** \brief Get centroid for a single voxel addressed by a PointT point.
196  * \param[in] point_arg point addressing a voxel in octree
197  * \param[out] voxel_centroid_arg centroid is written to this PointT reference
198  * \return "true" if voxel is found; "false" otherwise
199  */
200  bool
201  getVoxelCentroidAtPoint (const PointT& point_arg, PointT& voxel_centroid_arg) const;
202 
203  /** \brief Get centroid for a single voxel addressed by a PointT point from input cloud.
204  * \param[in] point_idx_arg point index from input cloud addressing a voxel in octree
205  * \param[out] voxel_centroid_arg centroid is written to this PointT reference
206  * \return "true" if voxel is found; "false" otherwise
207  */
208  inline bool
209  getVoxelCentroidAtPoint (const int& point_idx_arg, PointT& voxel_centroid_arg) const
210  {
211  // get centroid at point
212  return (this->getVoxelCentroidAtPoint (this->input_->points[point_idx_arg], voxel_centroid_arg));
213  }
214 
215  /** \brief Get PointT vector of centroids for all occupied voxels.
216  * \param[out] voxel_centroid_list_arg results are written to this vector of PointT elements
217  * \return number of occupied voxels
218  */
219  size_t
220  getVoxelCentroids (typename OctreePointCloud<PointT, LeafContainerT, BranchContainerT>::AlignedPointTVector &voxel_centroid_list_arg) const;
221 
222  /** \brief Recursively explore the octree and output a PointT vector of centroids for all occupied voxels.
223  * \param[in] branch_arg: current branch node
224  * \param[in] key_arg: current key
225  * \param[out] voxel_centroid_list_arg results are written to this vector of PointT elements
226  */
227  void
228  getVoxelCentroidsRecursive (const BranchNode* branch_arg,
229  OctreeKey& key_arg,
231 
232  };
233  }
234 }
235 
236 #include <pcl/octree/impl/octree_pointcloud_voxelcentroid.hpp>
237 
238 #endif
239