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_POINTCLOUD_H
00040 #define OCTREE_POINTCLOUD_H
00041
00042 #include "octree_base.h"
00043 #include "octree2buf_base.h"
00044
00045 #include <pcl/point_cloud.h>
00046 #include <pcl/point_types.h>
00047
00048 #include "octree_nodes.h"
00049 #include "octree_iterator.h"
00050
00051 #include <queue>
00052 #include <vector>
00053 #include <algorithm>
00054 #include <iostream>
00055
00056 namespace pcl
00057 {
00058 namespace octree
00059 {
00060
00062
00075
00076 template<typename PointT, typename LeafT = OctreeContainerDataTVector<int>,
00077 typename BranchT = OctreeContainerEmpty<int>,
00078 typename OctreeT = OctreeBase<int, LeafT, BranchT> >
00079
00080 class OctreePointCloud : public OctreeT
00081 {
00082
00083 friend class OctreeIteratorBase<int, OctreeT> ;
00084 friend class OctreeDepthFirstIterator<int, OctreeT> ;
00085 friend class OctreeBreadthFirstIterator<int, OctreeT> ;
00086 friend class OctreeLeafNodeIterator<int, OctreeT> ;
00087
00088 public:
00089 typedef OctreeT Base;
00090
00091 typedef typename OctreeT::LeafNode LeafNode;
00092 typedef typename OctreeT::BranchNode BranchNode;
00093
00094
00095 typedef OctreeDepthFirstIterator<int, OctreeT> Iterator;
00096 typedef const OctreeDepthFirstIterator<int, OctreeT> ConstIterator;
00097
00098 typedef OctreeLeafNodeIterator<int, OctreeT> LeafNodeIterator;
00099 typedef const OctreeLeafNodeIterator<int, OctreeT> ConstLeafNodeIterator;
00100
00101 typedef OctreeDepthFirstIterator<int, OctreeT> DepthFirstIterator;
00102 typedef const OctreeDepthFirstIterator<int, OctreeT> ConstDepthFirstIterator;
00103 typedef OctreeBreadthFirstIterator<int, OctreeT> BreadthFirstIterator;
00104 typedef const OctreeBreadthFirstIterator<int, OctreeT> ConstBreadthFirstIterator;
00105
00109 OctreePointCloud (const double resolution_arg);
00110
00112 virtual
00113 ~OctreePointCloud ();
00114
00115
00116 typedef boost::shared_ptr<std::vector<int> > IndicesPtr;
00117 typedef boost::shared_ptr<const std::vector<int> > IndicesConstPtr;
00118
00119 typedef pcl::PointCloud<PointT> PointCloud;
00120 typedef boost::shared_ptr<PointCloud> PointCloudPtr;
00121 typedef boost::shared_ptr<const PointCloud> PointCloudConstPtr;
00122
00123
00124 typedef OctreePointCloud<PointT, LeafT, OctreeBase<int, LeafT> > SingleBuffer;
00125 typedef OctreePointCloud<PointT, LeafT, Octree2BufBase<int, LeafT> > DoubleBuffer;
00126
00127
00128 typedef boost::shared_ptr<OctreePointCloud<PointT, LeafT, OctreeT> > Ptr;
00129 typedef boost::shared_ptr<const OctreePointCloud<PointT, LeafT, OctreeT> > ConstPtr;
00130
00131
00132 typedef std::vector<PointT, Eigen::aligned_allocator<PointT> > AlignedPointTVector;
00133
00138 inline void setInputCloud (const PointCloudConstPtr &cloud_arg,
00139 const IndicesConstPtr &indices_arg = IndicesConstPtr ())
00140 {
00141 assert(this->leafCount_==0);
00142
00143 input_ = cloud_arg;
00144 indices_ = indices_arg;
00145 }
00146
00150 inline IndicesConstPtr const getIndices () const
00151 {
00152 return (indices_);
00153 }
00154
00158 inline PointCloudConstPtr getInputCloud () const
00159 {
00160 return (input_);
00161 }
00162
00166 inline void setEpsilon (double eps)
00167 {
00168 epsilon_ = eps;
00169 }
00170
00172 inline double getEpsilon () const
00173 {
00174 return (epsilon_);
00175 }
00176
00180 inline void setResolution (double resolution_arg)
00181 {
00182
00183 assert( this->leafCount_ == 0);
00184
00185 resolution_ = resolution_arg;
00186
00187 getKeyBitSize ();
00188 }
00189
00193 inline double getResolution () const
00194 {
00195 return (resolution_);
00196 }
00197
00201 inline unsigned int getTreeDepth () const
00202 {
00203 return this->octreeDepth_;
00204 }
00205
00207 void
00208 addPointsFromInputCloud ();
00209
00214 void
00215 addPointFromCloud (const int pointIdx_arg, IndicesPtr indices_arg);
00216
00221 void
00222 addPointToCloud (const PointT& point_arg, PointCloudPtr cloud_arg);
00223
00229 void
00230 addPointToCloud (const PointT& point_arg, PointCloudPtr cloud_arg,
00231 IndicesPtr indices_arg);
00232
00237 bool
00238 isVoxelOccupiedAtPoint (const PointT& point_arg) const;
00239
00243 void deleteTree (bool freeMemory_arg = false)
00244 {
00245
00246 minX_ = minY_ = maxY_ = minZ_ = maxZ_ = 0;
00247 this->boundingBoxDefined_ = false;
00248
00249 OctreeT::deleteTree (freeMemory_arg);
00250 }
00251
00258 bool
00259 isVoxelOccupiedAtPoint (const double pointX_arg,
00260 const double pointY_arg, const double pointZ_arg) const;
00261
00266 bool
00267 isVoxelOccupiedAtPoint (const int& pointIdx_arg) const;
00268
00273 int
00274 getOccupiedVoxelCenters (
00275 AlignedPointTVector &voxelCenterList_arg) const;
00276
00287 int
00288 getApproxIntersectedVoxelCentersBySegment (
00289 const Eigen::Vector3f& origin, const Eigen::Vector3f& end,
00290 AlignedPointTVector &voxel_center_list, float precision = 0.2);
00291
00295 void
00296 deleteVoxelAtPoint (const PointT& point_arg);
00297
00301 void
00302 deleteVoxelAtPoint (const int& pointIdx_arg);
00303
00305
00307
00309 void
00310 defineBoundingBox ();
00311
00321 void
00322 defineBoundingBox (const double minX_arg, const double minY_arg,
00323 const double minZ_arg, const double maxX_arg, const double maxY_arg,
00324 const double maxZ_arg);
00325
00333 void
00334 defineBoundingBox (const double maxX_arg, const double maxY_arg,
00335 const double maxZ_arg);
00336
00342 void
00343 defineBoundingBox (const double cubeLen_arg);
00344
00354 void
00355 getBoundingBox (double& minX_arg, double& minY_arg, double& minZ_arg,
00356 double& maxX_arg, double& maxY_arg, double& maxZ_arg) const;
00357
00362 double
00363 getVoxelSquaredDiameter (unsigned int treeDepth_arg) const;
00364
00368 inline double getVoxelSquaredDiameter () const
00369 {
00370 return getVoxelSquaredDiameter (this->octreeDepth_);
00371 }
00372
00377 double
00378 getVoxelSquaredSideLen (unsigned int treeDepth_arg) const;
00379
00383 inline double getVoxelSquaredSideLen () const
00384 {
00385 return getVoxelSquaredSideLen (this->octreeDepth_);
00386 }
00387
00393 inline void getVoxelBounds (OctreeIteratorBase<int, OctreeT>& iterator,
00394 Eigen::Vector3f &min_pt, Eigen::Vector3f &max_pt)
00395 {
00396 this->genVoxelBoundsFromOctreeKey (iterator.getCurrentOctreeKey (),
00397 iterator.getCurrentOctreeDepth (), min_pt, max_pt);
00398 }
00399
00400 protected:
00401
00405 void
00406 addPointIdx (const int pointIdx_arg);
00407
00412 const PointT&
00413 getPointByIndex (const unsigned int index_arg) const;
00414
00419 LeafT*
00420 findLeafAtPoint (const PointT& point_arg) const;
00421
00423
00425
00427 void
00428 getKeyBitSize ();
00429
00433 void
00434 adoptBoundingBoxToPoint (const PointT& pointIdx_arg);
00435
00440 inline bool isPointWithinBoundingBox (const PointT& pointIdx_arg) const
00441 {
00442 return (! ( (pointIdx_arg.x < minX_) || (pointIdx_arg.y < minY_)
00443 || (pointIdx_arg.z < minZ_) || (pointIdx_arg.x >= maxX_)
00444 || (pointIdx_arg.y >= maxY_) || (pointIdx_arg.z >= maxZ_)));
00445 }
00446
00451 void
00452 genOctreeKeyforPoint (const PointT & point_arg,
00453 OctreeKey &key_arg) const;
00454
00461 void
00462 genOctreeKeyforPoint (const double pointX_arg, const double pointY_arg,
00463 const double pointZ_arg, OctreeKey & key_arg) const;
00464
00471 virtual bool
00472 genOctreeKeyForDataT (const int& data_arg, OctreeKey & key_arg) const;
00473
00478 void
00479 genLeafNodeCenterFromOctreeKey (const OctreeKey & key_arg,
00480 PointT& point_arg) const;
00481
00487 void
00488 genVoxelCenterFromOctreeKey (const OctreeKey & key_arg,
00489 unsigned int treeDepth_arg, PointT& point_arg) const;
00490
00497 void
00498 genVoxelBoundsFromOctreeKey (const OctreeKey & key_arg,
00499 unsigned int treeDepth_arg, Eigen::Vector3f &min_pt,
00500 Eigen::Vector3f &max_pt) const;
00501
00508 int
00509 getOccupiedVoxelCentersRecursive (const BranchNode* node_arg,
00510 const OctreeKey& key_arg,
00511 AlignedPointTVector &voxelCenterList_arg) const;
00512
00514
00516
00517 PointCloudConstPtr input_;
00518
00520 IndicesConstPtr indices_;
00521
00523 double epsilon_;
00524
00526 double resolution_;
00527
00528
00529 double minX_;
00530 double maxX_;
00531
00532 double minY_;
00533 double maxY_;
00534
00535 double minZ_;
00536 double maxZ_;
00537
00539 bool boundingBoxDefined_;
00540 };
00541 }
00542 }
00543
00544 #endif
00545