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
00326 virtual bool castRay(const point3d& origin, const point3d& direction, point3d& end,
00327 bool ignoreUnknownCells=false, double maxRange=-1.0) const;
00328
00340 virtual bool getRayIntersection(const point3d& origin, const point3d& direction, const point3d& center,
00341 point3d& intersection, double delta=0.0) const;
00342
00353 bool getNormals(const point3d& point, std::vector<point3d>& normals, bool unknownStatus=true) const;
00354
00355
00356
00358 void useBBXLimit(bool enable) { use_bbx_limit = enable; }
00359 bool bbxSet() const { return use_bbx_limit; }
00361 void setBBXMin (point3d& min);
00363 void setBBXMax (point3d& max);
00365 point3d getBBXMin () const { return bbx_min; }
00367 point3d getBBXMax () const { return bbx_max; }
00368 point3d getBBXBounds () const;
00369 point3d getBBXCenter () const;
00371 bool inBBX(const point3d& p) const;
00373 bool inBBX(const OcTreeKey& key) const;
00374
00375
00377 void enableChangeDetection(bool enable) { use_change_detection = enable; }
00379 void resetChangeDetection() { changed_keys.clear(); }
00380
00386 KeyBoolMap::const_iterator changedKeysBegin() {return changed_keys.begin();}
00387
00389 KeyBoolMap::const_iterator changedKeysEnd() {return changed_keys.end();}
00390
00391
00403 void computeUpdate(const Pointcloud& scan, const octomap::point3d& origin,
00404 KeySet& free_cells,
00405 KeySet& occupied_cells,
00406 double maxrange);
00407
00408
00421 void computeDiscreteUpdate(const Pointcloud& scan, const octomap::point3d& origin,
00422 KeySet& free_cells,
00423 KeySet& occupied_cells,
00424 double maxrange);
00425
00426
00427
00428
00434 std::istream& readBinaryData(std::istream &s);
00435
00443 std::istream& readBinaryNode(std::istream &s, NODE* node) const;
00444
00456 std::ostream& writeBinaryNode(std::ostream &s, const NODE* node) const;
00457
00462 std::ostream& writeBinaryData(std::ostream &s) const;
00463
00464
00470 void updateInnerOccupancy();
00471
00472
00474 virtual void integrateHit(NODE* occupancyNode) const;
00476 virtual void integrateMiss(NODE* occupancyNode) const;
00478 virtual void updateNodeLogOdds(NODE* occupancyNode, const float& update) const;
00479
00481 virtual void nodeToMaxLikelihood(NODE* occupancyNode) const;
00483 virtual void nodeToMaxLikelihood(NODE& occupancyNode) const;
00484
00485 protected:
00488 OccupancyOcTreeBase(double resolution, unsigned int tree_depth, unsigned int tree_max_val);
00489
00494 inline bool integrateMissOnRay(const point3d& origin, const point3d& end, bool lazy_eval = false);
00495
00496
00497
00498
00499 NODE* updateNodeRecurs(NODE* node, bool node_just_created, const OcTreeKey& key,
00500 unsigned int depth, const float& log_odds_update, bool lazy_eval = false);
00501
00502 NODE* setNodeValueRecurs(NODE* node, bool node_just_created, const OcTreeKey& key,
00503 unsigned int depth, const float& log_odds_value, bool lazy_eval = false);
00504
00505 void updateInnerOccupancyRecurs(NODE* node, unsigned int depth);
00506
00507 void toMaxLikelihoodRecurs(NODE* node, unsigned int depth, unsigned int max_depth);
00508
00509
00510 protected:
00511 bool use_bbx_limit;
00512 point3d bbx_min;
00513 point3d bbx_max;
00514 OcTreeKey bbx_min_key;
00515 OcTreeKey bbx_max_key;
00516
00517 bool use_change_detection;
00519 KeyBoolMap changed_keys;
00520
00521
00522 };
00523
00524 }
00525
00526 #include "octomap/OccupancyOcTreeBase.hxx"
00527
00528 #endif