Go to the documentation of this file.00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
00022
00023
00024
00025
00026
00027
00028
00029
00030
00031
00032
00033
00034
00035
00036
00037
00038
00039 #ifndef OCTREE_VOXELCENTROID_H
00040 #define OCTREE_VOXELCENTROID_H
00041
00042 #include "octree_pointcloud.h"
00043
00044 #include "octree_base.h"
00045 #include "octree2buf_base.h"
00046
00047 namespace pcl
00048 {
00049 namespace octree
00050 {
00051
00053
00061
00062 template<typename PointT, typename LeafT = OctreeContainerDataTVector<int> , typename BranchT = OctreeContainerEmpty<int> >
00063 class OctreePointCloudVoxelCentroid : public OctreePointCloud<PointT, LeafT, BranchT>
00064 {
00065
00066 public:
00067
00068
00069
00070 typedef typename OctreePointCloud<PointT, LeafT, BranchT>::AlignedPointTVector AlignedPointTVector;
00071
00075 OctreePointCloudVoxelCentroid (const double resolution_arg) :
00076 OctreePointCloud<PointT, LeafT, BranchT> (resolution_arg)
00077 {
00078 }
00079
00081 virtual
00082 ~OctreePointCloudVoxelCentroid ()
00083 {
00084 }
00085
00090 unsigned int
00091 getVoxelCentroids (AlignedPointTVector &voxelCentroidList_arg)
00092 {
00093
00094 size_t i;
00095 unsigned int pointCounter;
00096 OctreeKey keyC, keyP;
00097 PointT meanPoint;
00098 PointT idxPoint;
00099
00100 std::vector<int> indicesVector;
00101
00102 voxelCentroidList_arg.clear();
00103 voxelCentroidList_arg.reserve(this->leafCount_);
00104
00105
00106 this->serializeLeafs (indicesVector);
00107
00108
00109 keyP.x = keyP.y = keyP.z = std::numeric_limits<unsigned int>::max ();
00110 meanPoint.x = meanPoint.y = meanPoint.z = 0.0;
00111 pointCounter = 0;
00112
00113
00114 for (i = 0; i < indicesVector.size (); i++)
00115 {
00116 idxPoint = this->input_->points[indicesVector[i]];
00117
00118
00119 this->genOctreeKeyforPoint (idxPoint, keyC);
00120
00121 if (keyC == keyP)
00122 {
00123
00124 meanPoint.x += idxPoint.x;
00125 meanPoint.y += idxPoint.y;
00126 meanPoint.z += idxPoint.z;
00127
00128 pointCounter++;
00129 }
00130 else
00131 {
00132
00133 if (pointCounter > 0)
00134 {
00135 meanPoint.x /= static_cast<float> (pointCounter);
00136 meanPoint.y /= static_cast<float> (pointCounter);
00137 meanPoint.z /= static_cast<float> (pointCounter);
00138
00139 voxelCentroidList_arg.push_back (meanPoint);
00140 }
00141
00142
00143 meanPoint.x = idxPoint.x;
00144 meanPoint.y = idxPoint.y;
00145 meanPoint.z = idxPoint.z;
00146 pointCounter = 1;
00147
00148 keyP = keyC;
00149 }
00150 }
00151
00152
00153 if (pointCounter > 0)
00154 {
00155 meanPoint.x /= static_cast<float> (pointCounter);
00156 meanPoint.y /= static_cast<float> (pointCounter);
00157 meanPoint.z /= static_cast<float> (pointCounter);
00158
00159 voxelCentroidList_arg.push_back (meanPoint);
00160 }
00161
00162
00163 return (static_cast<unsigned int> (voxelCentroidList_arg.size ()));
00164 }
00165
00171 bool
00172 getVoxelCentroidAtPoint (const PointT& point_arg, PointT& voxelCentroid_arg)
00173 {
00174
00175 size_t i;
00176 unsigned int pointCounter;
00177 std::vector<int> indicesVector;
00178 PointT meanPoint;
00179 PointT idxPoint;
00180
00181 bool bResult;
00182
00183
00184 bResult = this->voxelSearch (point_arg, indicesVector);
00185
00186 if (bResult)
00187 {
00188 meanPoint.x = meanPoint.y = meanPoint.z = 0.0;
00189 pointCounter = 0;
00190
00191
00192 for (i = 0; i < indicesVector.size (); i++)
00193 {
00194 idxPoint = this->input_->points[indicesVector[i]];
00195
00196 meanPoint.x += idxPoint.x;
00197 meanPoint.y += idxPoint.y;
00198 meanPoint.z += idxPoint.z;
00199
00200 pointCounter++;
00201 }
00202
00203
00204 voxelCentroid_arg.x = meanPoint.x / static_cast<float> (pointCounter);
00205 voxelCentroid_arg.y = meanPoint.y / static_cast<float> (pointCounter);
00206 voxelCentroid_arg.z = meanPoint.z / static_cast<float> (pointCounter);
00207 }
00208
00209 return (bResult);
00210 }
00211
00217 inline bool
00218 getVoxelCentroidAtPoint (const int& pointIdx_arg, PointT& voxelCentroid_arg)
00219 {
00220
00221
00222 const PointT& point = this->input_->points[pointIdx_arg];
00223
00224
00225 return this->getVoxelCentroidAtPoint (point, voxelCentroid_arg);
00226
00227 }
00228
00229 };
00230
00231 }
00232
00233 }
00234
00235 #define PCL_INSTANTIATE_OctreePointCloudVoxelCentroid(T) template class PCL_EXPORTS pcl::octree::OctreePointCloudVoxelCentroid<T>;
00236
00237 #endif
00238