00001 #ifndef OCTOMAP_PCL_H 00002 #define OCTOMAP_PCL_H 00003 #include "octomap/OccupancyOcTreeBase.h" 00004 #include "OcTreeNodePCL.h" 00005 #include "octomap/ScanGraph.h" 00006 00007 namespace octomap { 00008 00014 class OcTreePCL : public OccupancyOcTreeBase <OcTreeNodePCL> { 00015 00016 public: 00017 static const int TREETYPE=3; 00018 std::vector < octomap::OcTreeNodePCL *> octree_node_list; 00019 public: 00020 00025 OcTreePCL(double _resolution); 00026 00036 void insertScan(const ScanNode& scan, double maxrange=-1., bool pruning = true); 00037 // -- Information --------------------------------- 00038 00040 unsigned int memoryUsage() const; 00041 00042 void calcNumThresholdedNodes(unsigned int& num_thresholded, unsigned int& num_other) const; 00043 00044 00045 00048 void toMaxLikelihood(); 00049 00050 // -- I/O ----------------------------------------- 00051 00053 00055 std::istream& readBinary(std::istream &s); 00056 00060 std::ostream& writeBinary(std::ostream &s); 00061 00064 std::ostream& writeBinaryConst(std::ostream &s) const; 00065 00066 00068 void readBinary(const std::string& filename); 00069 00072 void writeBinary(const std::string& filename); 00073 00076 void writeBinaryConst(const std::string& filename) const; 00077 00079 void insertOcTreeNodePCL(octomap::OcTreeNodePCL *); 00080 00082 octomap::OcTreeNodePCL * getOcTreeNodePCL(point3d c) const; 00083 00084 protected: 00085 00087 void insertScanUniform(const ScanNode& scan, double maxrange=-1.); 00088 00090 void toMaxLikelihoodRecurs(OcTreeNodePCL* node, unsigned int depth, unsigned int max_depth); 00091 00092 void calcNumThresholdedNodesRecurs (OcTreeNodePCL* node, 00093 unsigned int& num_thresholded, 00094 unsigned int& num_other) const; 00095 00096 }; 00097 00098 00099 } // end namespace 00100 00101 #endif