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 #ifndef OCTOMAP_OCCUPANCY_OCTREE_BASE_H
00035 #define OCTOMAP_OCCUPANCY_OCTREE_BASE_H
00036
00037
00038 #include <list>
00039 #include <stdlib.h>
00040 #include <vector>
00041
00042 #include "octomap_types.h"
00043 #include "octomap_utils.h"
00044 #include "OcTreeBaseImpl.h"
00045 #include "AbstractOccupancyOcTree.h"
00046
00047
00048 namespace octomap {
00049
00068 template <class NODE>
00069 class OccupancyOcTreeBase : public OcTreeBaseImpl<NODE,AbstractOccupancyOcTree> {
00070
00071 public:
00073 OccupancyOcTreeBase(double resolution);
00074 virtual ~OccupancyOcTreeBase();
00075
00077 OccupancyOcTreeBase(const OccupancyOcTreeBase<NODE>& rhs);
00078
00096 virtual void insertPointCloud(const Pointcloud& scan, const octomap::point3d& sensor_origin,
00097 double maxrange=-1., bool lazy_eval = false, bool discretize = false);
00098
00117 virtual void insertPointCloud(const Pointcloud& scan, const point3d& sensor_origin, const pose6d& frame_origin,
00118 double maxrange=-1., bool lazy_eval = false, bool discretize = false);
00119
00132 virtual void insertPointCloud(const ScanNode& scan, double maxrange=-1., bool lazy_eval = false, bool discretize = false);
00133
00135 OCTOMAP_DEPRECATED(virtual void insertScan(const Pointcloud& scan, const octomap::point3d& sensor_origin,
00136 double maxrange=-1., bool pruning=true, bool lazy_eval = false))
00137 {
00138 this->insertPointCloud(scan, sensor_origin, maxrange, lazy_eval);
00139 }
00140
00142 OCTOMAP_DEPRECATED(virtual void insertScan(const Pointcloud& scan, const point3d& sensor_origin,
00143 const pose6d& frame_origin, double maxrange=-1., bool pruning = true, bool lazy_eval = false))
00144 {
00145 this->insertPointCloud(scan, sensor_origin, frame_origin, maxrange, lazy_eval);
00146 }
00147
00149 OCTOMAP_DEPRECATED(virtual void insertScan(const ScanNode& scan, double maxrange=-1., bool pruning = true, bool lazy_eval = false)){
00150 this->insertPointCloud(scan, maxrange, lazy_eval);
00151 }
00152
00154 OCTOMAP_DEPRECATED( virtual void insertScanNaive(const Pointcloud& scan, const point3d& sensor_origin, double maxrange, bool lazy_eval = false)){
00155 this->insertPointCloudRays(scan, sensor_origin, maxrange, lazy_eval);
00156 }
00157
00170 virtual void insertPointCloudRays(const Pointcloud& scan, const point3d& sensor_origin, double maxrange = -1., bool lazy_eval = false);
00171
00182 virtual NODE* setNodeValue(const OcTreeKey& key, float log_odds_value, bool lazy_eval = false);
00183
00194 virtual NODE* setNodeValue(const point3d& value, float log_odds_value, bool lazy_eval = false);
00195
00208 virtual NODE* setNodeValue(double x, double y, double z, float log_odds_value, bool lazy_eval = false);
00209
00220 virtual NODE* updateNode(const OcTreeKey& key, float log_odds_update, bool lazy_eval = false);
00221
00232 virtual NODE* updateNode(const point3d& value, float log_odds_update, bool lazy_eval = false);
00233
00246 virtual NODE* updateNode(double x, double y, double z, float log_odds_update, bool lazy_eval = false);
00247
00257 virtual NODE* updateNode(const OcTreeKey& key, bool occupied, bool lazy_eval = false);
00258
00269 virtual NODE* updateNode(const point3d& value, bool occupied, bool lazy_eval = false);
00270
00283 virtual NODE* updateNode(double x, double y, double z, bool occupied, bool lazy_eval = false);
00284
00285
00291 virtual void toMaxLikelihood();
00292
00306 virtual bool insertRay(const point3d& origin, const point3d& end, double maxrange=-1.0, bool lazy_eval = false);
00307
00323 virtual bool castRay(const point3d& origin, const point3d& direction, point3d& end,
00324 bool ignoreUnknownCells=false, double maxRange=-1.0) const;
00325
00337 virtual bool getRayIntersection(const point3d& origin, const point3d& direction, const point3d& center,
00338 point3d& intersection, double delta=0.0) const;
00339
00350 bool getNormals(const point3d& point, std::vector<point3d>& normals, bool unknownStatus=true) const;
00351
00352
00353
00355 void useBBXLimit(bool enable) { use_bbx_limit = enable; }
00356 bool bbxSet() const { return use_bbx_limit; }
00358 void setBBXMin (point3d& min);
00360 void setBBXMax (point3d& max);
00362 point3d getBBXMin () const { return bbx_min; }
00364 point3d getBBXMax () const { return bbx_max; }
00365 point3d getBBXBounds () const;
00366 point3d getBBXCenter () const;
00368 bool inBBX(const point3d& p) const;
00370 bool inBBX(const OcTreeKey& key) const;
00371
00372
00374 void enableChangeDetection(bool enable) { use_change_detection = enable; }
00376 void resetChangeDetection() { changed_keys.clear(); }
00377
00383 KeyBoolMap::const_iterator changedKeysBegin() {return changed_keys.begin();}
00384
00386 KeyBoolMap::const_iterator changedKeysEnd() {return changed_keys.end();}
00387
00388
00400 void computeUpdate(const Pointcloud& scan, const octomap::point3d& origin,
00401 KeySet& free_cells,
00402 KeySet& occupied_cells,
00403 double maxrange);
00404
00405
00418 void computeDiscreteUpdate(const Pointcloud& scan, const octomap::point3d& origin,
00419 KeySet& free_cells,
00420 KeySet& occupied_cells,
00421 double maxrange);
00422
00423
00424
00425
00431 std::istream& readBinaryData(std::istream &s);
00432
00440 std::istream& readBinaryNode(std::istream &s, NODE* node) const;
00441
00453 std::ostream& writeBinaryNode(std::ostream &s, const NODE* node) const;
00454
00459 std::ostream& writeBinaryData(std::ostream &s) const;
00460
00461
00467 void updateInnerOccupancy();
00468
00469
00471 virtual void integrateHit(NODE* occupancyNode) const;
00473 virtual void integrateMiss(NODE* occupancyNode) const;
00475 virtual void updateNodeLogOdds(NODE* occupancyNode, const float& update) const;
00476
00478 virtual void nodeToMaxLikelihood(NODE* occupancyNode) const;
00480 virtual void nodeToMaxLikelihood(NODE& occupancyNode) const;
00481
00482 protected:
00485 OccupancyOcTreeBase(double resolution, unsigned int tree_depth, unsigned int tree_max_val);
00486
00491 inline bool integrateMissOnRay(const point3d& origin, const point3d& end, bool lazy_eval = false);
00492
00493
00494
00495
00496 NODE* updateNodeRecurs(NODE* node, bool node_just_created, const OcTreeKey& key,
00497 unsigned int depth, const float& log_odds_update, bool lazy_eval = false);
00498
00499 NODE* setNodeValueRecurs(NODE* node, bool node_just_created, const OcTreeKey& key,
00500 unsigned int depth, const float& log_odds_value, bool lazy_eval = false);
00501
00502 void updateInnerOccupancyRecurs(NODE* node, unsigned int depth);
00503
00504 void toMaxLikelihoodRecurs(NODE* node, unsigned int depth, unsigned int max_depth);
00505
00506
00507 protected:
00508 bool use_bbx_limit;
00509 point3d bbx_min;
00510 point3d bbx_max;
00511 OcTreeKey bbx_min_key;
00512 OcTreeKey bbx_max_key;
00513
00514 bool use_change_detection;
00516 KeyBoolMap changed_keys;
00517
00518
00519 };
00520
00521 }
00522
00523 #include "octomap/OccupancyOcTreeBase.hxx"
00524
00525 #endif