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