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