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

Fixed unknown lines for octomap 2D grid projection. (https://github.com/introlab/rtabmap_ros/issues/684)

Browse Source
This commit is contained in:
matlabbe
2021-11-30 20:34:51 -05:00
parent 67710ef94c
commit baae713471
3 changed files with 30 additions and 7 deletions
Download Patch File Download Diff File Expand all files Collapse all files

2
corelib/include/rtabmap/core/OctoMap.h
View File

@@ -74,6 +74,8 @@ public:
void expandNode();
bool createChild(unsigned int i);
void updateOccupancyTypeChildren();
private:
int nodeRefId_;
int type_; // -1=undefined, 0=empty, 100=obstacle, 1=ground

31
corelib/src/OctoMap.cpp
View File

@@ -106,6 +106,23 @@ bool RtabmapColorOcTreeNode::createChild(unsigned int i) {
#endif
}
void RtabmapColorOcTreeNode::updateOccupancyTypeChildren()
{
if (children != NULL){
int type = kTypeUnknown;
for (int i=0; i<8 && type != kTypeObstacle; i++) {
RtabmapColorOcTreeNode* child = static_cast<RtabmapColorOcTreeNode*>(children[i]);
if (child != NULL && child->getOccupancyType() >= kTypeEmpty) {
if(type == kTypeUnknown) {
type = child->getOccupancyType();
}
}
}
type_ = type;
}
}
RtabmapColorOcTree::RtabmapColorOcTree(double resolution)
: OccupancyOcTreeBase<RtabmapColorOcTreeNode>(resolution) {
RtabmapColorOcTreeMemberInit.ensureLinking();
@@ -231,6 +248,7 @@ void RtabmapColorOcTree::updateInnerOccupancyRecurs(RtabmapColorOcTreeNode* node
}
node->updateOccupancyChildren();
node->updateColorChildren();
node->updateOccupancyTypeChildren();
}
#else
// only recurse and update for inner nodes:
@@ -245,6 +263,7 @@ void RtabmapColorOcTree::updateInnerOccupancyRecurs(RtabmapColorOcTreeNode* node
}
node->updateOccupancyChildren();
node->updateColorChildren();
node->updateOccupancyTypeChildren();
}
#endif
}
@@ -1209,21 +1228,23 @@ cv::Mat OctoMap::createProjectionMap(float & xMin, float & yMin, float & gridCel
int oi=0;
cv::Vec2f * oPtr = obstaclesMat.ptr<cv::Vec2f>(0,0);
cv::Vec2f * gPtr = groundMat.ptr<cv::Vec2f>(0,0);
float halfCellSize = octree_->getNodeSize(treeDepth)/2.0f;
for (RtabmapColorOcTree::iterator it = octree_->begin(treeDepth); it != octree_->end(); ++it)
{
octomap::point3d pt = octree_->keyToCoord(it.getKey());
if(octree_->isNodeOccupied(*it) && it->getOccupancyType() == RtabmapColorOcTreeNode::kTypeObstacle)
if(octree_->isNodeOccupied(*it) &&
it->getOccupancyType() == RtabmapColorOcTreeNode::kTypeObstacle)
{
// projected on ground
oPtr[oi][0] = pt.x();
oPtr[oi][1] = pt.y();
oPtr[oi][0] = pt.x()-halfCellSize;
oPtr[oi][1] = pt.y()-halfCellSize;
++oi;
}
else
{
// projected on ground
gPtr[gi][0] = pt.x();
gPtr[gi][1] = pt.y();
gPtr[gi][0] = pt.x()-halfCellSize;
gPtr[gi][1] = pt.y()-halfCellSize;
++gi;
}
}

4
corelib/src/util3d_mapping.cpp
View File

@@ -330,8 +330,8 @@ cv::Mat create2DMapFromOccupancyLocalMaps(
{
//Get map size
float margin = cellSize*10.0f;
xMin = minX-margin;
yMin = minY-margin;
xMin = minX-margin-cellSize/2.0f;
yMin = minY-margin-cellSize/2.0f;
float xMax = maxX+margin;
float yMax = maxY+margin;
if(fabs((yMax - yMin) / cellSize) > 30000 || // Max 1.5Km/1.5Km at 5 cm/cell -> 900MB