i/rtabmap
1
0
Fork 0
mirror of https://github.com/introlab/rtabmap.git synced 2026-09-03 01:50:24 +08:00
Code Issues Packages Projects Releases Wiki Activity

OctoMap added frontier cloud output

Browse Source
This commit is contained in:
Louis Petit
2019-12-17 17:31:37 -05:00
parent af9e113112
commit af2b221958
2 changed files with 27 additions and 4 deletions
Download Patch File Download Diff File Expand all files Collapse all files

28
corelib/src/OctoMap.cpp
View File

@@ -940,7 +940,8 @@ pcl::PointCloud<pcl::PointXYZRGB>::Ptr OctoMap::createCloud(
std::vector<int> * obstacleIndices,
std::vector<int> * emptyIndices,
std::vector<int> * groundIndices,
bool originalRefPoints) const
bool originalRefPoints,
std::vector<int> * frontierIndices) const
{
UASSERT(treeDepth <= octree_->getTreeDepth());
pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud(new pcl::PointCloud<pcl::PointXYZRGB>);
@@ -955,6 +956,10 @@ pcl::PointCloud<pcl::PointXYZRGB>::Ptr OctoMap::createCloud(
{
emptyIndices->resize(octree_->size());
}
if(frontierIndices)
{
frontierIndices->resize(octree_->size());
}
if(groundIndices)
{
groundIndices->resize(octree_->size());
@@ -972,6 +977,7 @@ pcl::PointCloud<pcl::PointXYZRGB>::Ptr OctoMap::createCloud(
int oi=0;
int si=0;
int ei=0;
int fi=0;
int gi=0;
float halfCellSize = octree_->getNodeSize(treeDepth)/2.0f;
for (RtabmapColorOcTree::iterator it = octree_->begin(treeDepth); it != octree_->end(); ++it)
@@ -1022,9 +1028,19 @@ pcl::PointCloud<pcl::PointXYZRGB>::Ptr OctoMap::createCloud(
++oi;
}
else if(!octree_->isNodeOccupied(*it) && (emptyIndices != 0 || addAllPoints))
else if(!octree_->isNodeOccupied(*it) && (emptyIndices != 0 || addAllPoints || frontierIndices !=0))
{
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 ?
{
//unknown neighbor FACE cell
frontierIndices->at(fi++) = oi;
}
(*cloud)[oi] = pcl::PointXYZRGB(it->getColor().r, it->getColor().g, it->getColor().b);
(*cloud)[oi].x = pt.x()-halfCellSize;
(*cloud)[oi].y = pt.y()-halfCellSize;
@@ -1033,8 +1049,9 @@ pcl::PointCloud<pcl::PointXYZRGB>::Ptr OctoMap::createCloud(
{
emptyIndices->at(ei++) = oi;
}
++oi;
}
}
}
cloud->resize(oi);
@@ -1048,6 +1065,11 @@ pcl::PointCloud<pcl::PointXYZRGB>::Ptr OctoMap::createCloud(
emptyIndices->resize(ei);
UDEBUG("empty=%d", ei);
}
if(frontierIndices)
{
frontierIndices->resize(fi);
UDEBUG("frontier=%d", fi);
}
if(groundIndices)
{
groundIndices->resize(gi);