Program Listing for File dynamicEDTOctomap.hxx
↰ Return to documentation for file (/tmp/ws/src/octomap/dynamicEDT3D/include/dynamicEDT3D/dynamicEDTOctomap.hxx)
/*
* Copyright (c) 2011-2012, C. Sprunk, B. Lau, W. Burgard, University of Freiburg
* All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions are met:
*
* * Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* * Redistributions in binary form must reproduce the above copyright
* notice, this list of conditions and the following disclaimer in the
* documentation and/or other materials provided with the distribution.
* * Neither the name of the University of Freiburg nor the names of its
* contributors may be used to endorse or promote products derived from
* this software without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
* AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
* IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
* ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
* LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
* CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
* SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
* INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
* CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
* ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*/
template <class TREE>
float DynamicEDTOctomapBase<TREE>::distanceValue_Error = -1.0;
template <class TREE>
int DynamicEDTOctomapBase<TREE>::distanceInCellsValue_Error = -1;
template <class TREE>
DynamicEDTOctomapBase<TREE>::DynamicEDTOctomapBase(float maxdist, TREE* _octree, octomap::point3d bbxMin, octomap::point3d bbxMax, bool treatUnknownAsOccupied)
: DynamicEDT3D(((int) (maxdist/_octree->getResolution()+1)*((int) (maxdist/_octree->getResolution()+1)))), octree(_octree), unknownOccupied(treatUnknownAsOccupied)
{
treeDepth = octree->getTreeDepth();
treeResolution = octree->getResolution();
initializeOcTree(bbxMin, bbxMax);
octree->enableChangeDetection(true);
}
template <class TREE>
DynamicEDTOctomapBase<TREE>::~DynamicEDTOctomapBase() {
}
template <class TREE>
void DynamicEDTOctomapBase<TREE>::update(bool updateRealDist){
for(octomap::KeyBoolMap::const_iterator it = octree->changedKeysBegin(), end=octree->changedKeysEnd(); it!=end; ++it){
//the keys in this list all go down to the lowest level!
octomap::OcTreeKey key = it->first;
//ignore changes outside of bounding box
if(key[0] < boundingBoxMinKey[0] || key[1] < boundingBoxMinKey[1] || key[2] < boundingBoxMinKey[2])
continue;
if(key[0] > boundingBoxMaxKey[0] || key[1] > boundingBoxMaxKey[1] || key[2] > boundingBoxMaxKey[2])
continue;
typename TREE::NodeType* node = octree->search(key);
assert(node);
//"node" is not necessarily at lowest level, BUT: the occupancy value of this node
//has to be the same as of the node indexed by the key *it
updateMaxDepthLeaf(key, octree->isNodeOccupied(node));
}
octree->resetChangeDetection();
DynamicEDT3D::update(updateRealDist);
}
template <class TREE>
void DynamicEDTOctomapBase<TREE>::initializeOcTree(octomap::point3d bbxMin, octomap::point3d bbxMax){
boundingBoxMinKey = octree->coordToKey(bbxMin);
boundingBoxMaxKey = octree->coordToKey(bbxMax);
offsetX = -boundingBoxMinKey[0];
offsetY = -boundingBoxMinKey[1];
offsetZ = -boundingBoxMinKey[2];
int _sizeX = boundingBoxMaxKey[0] - boundingBoxMinKey[0] + 1;
int _sizeY = boundingBoxMaxKey[1] - boundingBoxMinKey[1] + 1;
int _sizeZ = boundingBoxMaxKey[2] - boundingBoxMinKey[2] + 1;
initializeEmpty(_sizeX, _sizeY, _sizeZ, false);
if(unknownOccupied == false){
for(typename TREE::leaf_bbx_iterator it = octree->begin_leafs_bbx(bbxMin,bbxMax), end=octree->end_leafs_bbx(); it!= end; ++it){
if(octree->isNodeOccupied(*it)){
int nodeDepth = it.getDepth();
if( nodeDepth == treeDepth){
insertMaxDepthLeafAtInitialize(it.getKey());
} else {
int cubeSize = 1 << (treeDepth - nodeDepth);
octomap::OcTreeKey key=it.getIndexKey();
for(int dx = 0; dx < cubeSize; dx++)
for(int dy = 0; dy < cubeSize; dy++)
for(int dz = 0; dz < cubeSize; dz++){
unsigned short int tmpx = key[0]+dx;
unsigned short int tmpy = key[1]+dy;
unsigned short int tmpz = key[2]+dz;
if(boundingBoxMinKey[0] > tmpx || boundingBoxMinKey[1] > tmpy || boundingBoxMinKey[2] > tmpz)
continue;
if(boundingBoxMaxKey[0] < tmpx || boundingBoxMaxKey[1] < tmpy || boundingBoxMaxKey[2] < tmpz)
continue;
insertMaxDepthLeafAtInitialize(octomap::OcTreeKey(tmpx, tmpy, tmpz));
}
}
}
}
} else {
octomap::OcTreeKey key;
for(int dx=0; dx<sizeX; dx++){
key[0] = boundingBoxMinKey[0] + dx;
for(int dy=0; dy<sizeY; dy++){
key[1] = boundingBoxMinKey[1] + dy;
for(int dz=0; dz<sizeZ; dz++){
key[2] = boundingBoxMinKey[2] + dz;
typename TREE::NodeType* node = octree->search(key);
if(!node || octree->isNodeOccupied(node)){
insertMaxDepthLeafAtInitialize(key);
}
}
}
}
}
}
template <class TREE>
void DynamicEDTOctomapBase<TREE>::insertMaxDepthLeafAtInitialize(octomap::OcTreeKey key){
bool isSurrounded = true;
for(int dx=-1; dx<=1; dx++)
for(int dy=-1; dy<=1; dy++)
for(int dz=-1; dz<=1; dz++){
if(dx==0 && dy==0 && dz==0)
continue;
typename TREE::NodeType* node = octree->search(octomap::OcTreeKey(key[0]+dx, key[1]+dy, key[2]+dz));
if((!unknownOccupied && node==NULL) || ((node!=NULL) && (octree->isNodeOccupied(node)==false))){
isSurrounded = false;
break;
}
}
if(isSurrounded){
//obstacles that are surrounded by obstacles do not need to be put in the queues,
//hence this initialization
dataCell c;
int x = key[0]+offsetX;
int y = key[1]+offsetY;
int z = key[2]+offsetZ;
c.obstX = x;
c.obstY = y;
c.obstZ = z;
c.sqdist = 0;
c.dist = 0.0;
c.queueing = fwProcessed;
c.needsRaise = false;
data[x][y][z] = c;
} else {
setObstacle(key[0]+offsetX, key[1]+offsetY, key[2]+offsetZ);
}
}
template <class TREE>
void DynamicEDTOctomapBase<TREE>::updateMaxDepthLeaf(octomap::OcTreeKey& key, bool occupied){
if(occupied)
setObstacle(key[0]+offsetX, key[1]+offsetY, key[2]+offsetZ);
else
removeObstacle(key[0]+offsetX, key[1]+offsetY, key[2]+offsetZ);
}
template <class TREE>
void DynamicEDTOctomapBase<TREE>::worldToMap(const octomap::point3d &p, int &x, int &y, int &z) const {
octomap::OcTreeKey key = octree->coordToKey(p);
x = key[0] + offsetX;
y = key[1] + offsetY;
z = key[2] + offsetZ;
}
template <class TREE>
void DynamicEDTOctomapBase<TREE>::mapToWorld(int x, int y, int z, octomap::point3d &p) const {
p = octree->keyToCoord(octomap::OcTreeKey(x-offsetX, y-offsetY, z-offsetZ));
}
template <class TREE>
void DynamicEDTOctomapBase<TREE>::mapToWorld(int x, int y, int z, octomap::OcTreeKey &key) const {
key = octomap::OcTreeKey(x-offsetX, y-offsetY, z-offsetZ);
}
template <class TREE>
void DynamicEDTOctomapBase<TREE>::getDistanceAndClosestObstacle(const octomap::point3d& p, float &distance, octomap::point3d& closestObstacle) const {
int x,y,z;
worldToMap(p, x, y, z);
if(x>=0 && x<sizeX && y>=0 && y<sizeY && z>=0 && z<sizeZ){
dataCell c= data[x][y][z];
distance = c.dist*treeResolution;
if(c.obstX != invalidObstData){
mapToWorld(c.obstX, c.obstY, c.obstZ, closestObstacle);
} else {
//If we are at maxDist, it can very well be that there is no valid closest obstacle data for this cell, this is not an error.
}
} else {
distance = distanceValue_Error;
}
}
template <class TREE>
void DynamicEDTOctomapBase<TREE>::getDistanceAndClosestObstacle_unsafe(const octomap::point3d& p, float &distance, octomap::point3d& closestObstacle) const {
int x,y,z;
worldToMap(p, x, y, z);
dataCell c= data[x][y][z];
distance = c.dist*treeResolution;
if(c.obstX != invalidObstData){
mapToWorld(c.obstX, c.obstY, c.obstZ, closestObstacle);
} else {
//If we are at maxDist, it can very well be that there is no valid closest obstacle data for this cell, this is not an error.
}
}
template <class TREE>
float DynamicEDTOctomapBase<TREE>::getDistance(const octomap::point3d& p) const {
int x,y,z;
worldToMap(p, x, y, z);
if(x>=0 && x<sizeX && y>=0 && y<sizeY && z>=0 && z<sizeZ){
return data[x][y][z].dist*treeResolution;
} else {
return distanceValue_Error;
}
}
template <class TREE>
float DynamicEDTOctomapBase<TREE>::getDistance_unsafe(const octomap::point3d& p) const {
int x,y,z;
worldToMap(p, x, y, z);
return data[x][y][z].dist*treeResolution;
}
template <class TREE>
float DynamicEDTOctomapBase<TREE>::getDistance(const octomap::OcTreeKey& k) const {
int x = k[0] + offsetX;
int y = k[1] + offsetY;
int z = k[2] + offsetZ;
if(x>=0 && x<sizeX && y>=0 && y<sizeY && z>=0 && z<sizeZ){
return data[x][y][z].dist*treeResolution;
} else {
return distanceValue_Error;
}
}
template <class TREE>
float DynamicEDTOctomapBase<TREE>::getDistance_unsafe(const octomap::OcTreeKey& k) const {
int x = k[0] + offsetX;
int y = k[1] + offsetY;
int z = k[2] + offsetZ;
return data[x][y][z].dist*treeResolution;
}
template <class TREE>
int DynamicEDTOctomapBase<TREE>::getSquaredDistanceInCells(const octomap::point3d& p) const {
int x,y,z;
worldToMap(p, x, y, z);
if(x>=0 && x<sizeX && y>=0 && y<sizeY && z>=0 && z<sizeZ){
return data[x][y][z].sqdist;
} else {
return distanceInCellsValue_Error;
}
}
template <class TREE>
int DynamicEDTOctomapBase<TREE>::getSquaredDistanceInCells_unsafe(const octomap::point3d& p) const {
int x,y,z;
worldToMap(p, x, y, z);
return data[x][y][z].sqdist;
}
template <class TREE>
bool DynamicEDTOctomapBase<TREE>::checkConsistency() const {
for(octomap::KeyBoolMap::const_iterator it = octree->changedKeysBegin(), end=octree->changedKeysEnd(); it!=end; ++it){
//std::cerr<<"Cannot check consistency, you must execute the update() method first."<<std::endl;
return false;
}
for(int x=0; x<sizeX; x++){
for(int y=0; y<sizeY; y++){
for(int z=0; z<sizeZ; z++){
octomap::point3d point;
mapToWorld(x,y,z,point);
typename TREE::NodeType* node = octree->search(point);
bool mapOccupied = isOccupied(x,y,z);
bool treeOccupied = false;
if(node){
treeOccupied = octree->isNodeOccupied(node);
} else {
if(unknownOccupied)
treeOccupied = true;
}
if(mapOccupied != treeOccupied){
//std::cerr<<"OCCUPANCY MISMATCH BETWEEN TREE AND MAP at "<<x<<","<<y<<","<<z<<std::endl;
//std::cerr<<"Tree "<<treeOccupied<<std::endl;
//std::cerr<<"Map "<<mapOccupied<<std::endl;
return false;
}
}
}
}
return true;
}