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: fixed not dense empty cells

Browse Source
This commit is contained in:
matlabbe
2018-12-07 19:50:15 -05:00
parent 200ec8e5db
commit de32e53868
2 changed files with 13 additions and 9 deletions
Download Patch File Download Diff File Expand all files Collapse all files

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

@@ -224,7 +224,6 @@ private:
std::map<int, cv::Point3f> cacheViewPoints_; std::map<int, cv::Point3f> cacheViewPoints_;
RtabmapColorOcTree * octree_; RtabmapColorOcTree * octree_;
std::map<int, Transform> addedNodes_; std::map<int, Transform> addedNodes_;
octomap::KeyRay keyRay_;
bool hasColor_; bool hasColor_;
bool fullUpdate_; bool fullUpdate_;
float updateError_; float updateError_;

19
corelib/src/OctoMap.cpp
View File

@@ -339,7 +339,6 @@ void OctoMap::clear()
cacheClouds_.clear(); cacheClouds_.clear();
cacheViewPoints_.clear(); cacheViewPoints_.clear();
addedNodes_.clear(); addedNodes_.clear();
keyRay_ = octomap::KeyRay();
hasColor_ = false; hasColor_ = false;
minValues_[0] = minValues_[1] = minValues_[2] = 0.0; minValues_[0] = minValues_[1] = minValues_[2] = 0.0;
maxValues_[0] = maxValues_[1] = maxValues_[2] = 0.0; maxValues_[0] = maxValues_[1] = maxValues_[2] = 0.0;
@@ -410,6 +409,7 @@ void OctoMap::update(const std::map<int, Transform> & poses)
UDEBUG("Updated pose for node %d is not found, some points may not be copied. Use negative ids to just update cell values without adding new ones.", jter->first); UDEBUG("Updated pose for node %d is not found, some points may not be copied. Use negative ids to just update cell values without adding new ones.", jter->first);
} }
} }
if(graphOptimized || graphChanged) if(graphOptimized || graphChanged)
{ {
if(graphChanged) if(graphChanged)
@@ -429,7 +429,6 @@ void OctoMap::update(const std::map<int, Transform> & poses)
// clear all but keep cache // clear all but keep cache
octree_->clear(); octree_->clear();
addedNodes_.clear(); addedNodes_.clear();
keyRay_ = octomap::KeyRay();
hasColor_ = false; hasColor_ = false;
} }
else else
@@ -649,11 +648,12 @@ void OctoMap::update(const std::map<int, Transform> & poses)
} }
// only clear space (ground points) // only clear space (ground points)
octomap::KeyRay keyRay;
if (computeRays && if (computeRays &&
(iter->first < 0 || iter->first>lastId) && (iter->first < 0 || iter->first>lastId) &&
octree_->computeRayKeys(sensorOrigin, point, keyRay_)) octree_->computeRayKeys(sensorOrigin, point, keyRay))
{ {
free_cells.insert(keyRay_.begin(), keyRay_.end()); free_cells.insert(keyRay.begin(), keyRay.end());
} }
} }
UDEBUG("%d: ground cells=%d free cells=%d", iter->first, (int)maxGroundPts, (int)free_cells.size()); UDEBUG("%d: ground cells=%d free cells=%d", iter->first, (int)maxGroundPts, (int)free_cells.size());
@@ -733,11 +733,12 @@ void OctoMap::update(const std::map<int, Transform> & poses)
} }
// free cells // free cells
octomap::KeyRay keyRay;
if (computeRays && if (computeRays &&
(iter->first < 0 || iter->first>lastId) && (iter->first < 0 || iter->first>lastId) &&
octree_->computeRayKeys(sensorOrigin, point, keyRay_)) octree_->computeRayKeys(sensorOrigin, point, keyRay))
{ {
free_cells.insert(keyRay_.begin(), keyRay_.end()); free_cells.insert(keyRay.begin(), keyRay.end());
} }
} }
UDEBUG("%d: occupied cells=%d free cells=%d", iter->first, (int)maxObstaclePts, (int)free_cells.size()); UDEBUG("%d: occupied cells=%d free cells=%d", iter->first, (int)maxObstaclePts, (int)free_cells.size());
@@ -756,7 +757,7 @@ void OctoMap::update(const std::map<int, Transform> & poses)
} }
} }
RtabmapColorOcTreeNode * n = octree_->updateNode(*it, false); RtabmapColorOcTreeNode * n = octree_->updateNode(*it, false, orderedPoses.size() == 1);
if(n && n->getOccupancyType() == RtabmapColorOcTreeNode::kTypeUnknown) if(n && n->getOccupancyType() == RtabmapColorOcTreeNode::kTypeUnknown)
{ {
n->setOccupancyType(RtabmapColorOcTreeNode::kTypeEmpty); n->setOccupancyType(RtabmapColorOcTreeNode::kTypeEmpty);
@@ -822,10 +823,14 @@ void OctoMap::update(const std::map<int, Transform> & poses)
} }
} }
} }
//octree_->updateInnerOccupancy();
} }
// compress map // compress map
//if(orderedPoses.size() > 1)
//{
// octree_->prune(); // octree_->prune();
//}
// ignore negative ids as they are temporary clouds // ignore negative ids as they are temporary clouds
if(iter->first > 0) if(iter->first > 0)