i/rtabmap
1
0
Fork 0
mirror of https://github.com/introlab/rtabmap.git synced 2026-09-02 17:40:23 +08:00
Code Issues Packages Projects Releases Wiki Activity

OctoMap: fixed frontier with higher tree depth

Browse Source
This commit is contained in:
matlabbe
2019-12-20 15:56:52 -05:00
parent af2b221958
commit 7d1a97696b
1 changed files with 4 additions and 3 deletions
Download Patch File Download Diff File Expand all files Collapse all files

7
corelib/src/OctoMap.cpp
View File

@@ -1032,9 +1032,10 @@ pcl::PointCloud<pcl::PointXYZRGB>::Ptr OctoMap::createCloud(
{
octomap::point3d pt = octree_->keyToCoord(it.getKey());
if(frontierIndices !=0 && (!octree_->search( pt.x()+octree_->getNodeSize(treeDepth), pt.y(), pt.z() ) || !octree_->search( pt.x()-octree_->getNodeSize(treeDepth), pt.y(), pt.z() )
|| !octree_->search( pt.x(), pt.y()+octree_->getNodeSize(treeDepth), pt.z() ) || !octree_->search( pt.x(), pt.y()-octree_->getNodeSize(treeDepth), pt.z() )
|| !octree_->search( pt.x(), pt.y(), pt.z()+octree_->getNodeSize(treeDepth) ) || !octree_->search( pt.x(), pt.y(), pt.z()-octree_->getNodeSize(treeDepth) ) )) //ajouter 1 au key ?
if(frontierIndices !=0 &&
(!octree_->search( pt.x()+octree_->getNodeSize(treeDepth), pt.y(), pt.z(), treeDepth) || !octree_->search( pt.x()-octree_->getNodeSize(treeDepth), pt.y(), pt.z(), treeDepth) ||
!octree_->search( pt.x(), pt.y()+octree_->getNodeSize(treeDepth), pt.z(), treeDepth) || !octree_->search( pt.x(), pt.y()-octree_->getNodeSize(treeDepth), pt.z(), treeDepth) ||
!octree_->search( pt.x(), pt.y(), pt.z()+octree_->getNodeSize(treeDepth), treeDepth) || !octree_->search( pt.x(), pt.y(), pt.z()-octree_->getNodeSize(treeDepth), treeDepth) )) //ajouter 1 au key ?
{
//unknown neighbor FACE cell
frontierIndices->at(fi++) = oi;