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

Grid/OctoMap: updated update() interface to return boolean to know if the map has been updated

Browse Source
This commit is contained in:
matlabbe
2019-03-19 14:10:30 -04:00
parent c0a7c3a344
commit e8e3649f24
4 changed files with 822 additions and 812 deletions
Download Patch File Download Diff File Expand all files Collapse all files

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

@@ -97,7 +97,7 @@ public:
const cv::Mat & ground, const cv::Mat & ground,
const cv::Mat & obstacles, const cv::Mat & obstacles,
const cv::Mat & empty); const cv::Mat & empty);
void update(const std::map<int, Transform> & poses); bool update(const std::map<int, Transform> & poses); // return true if map has changed
cv::Mat getMap(float & xMin, float & yMin) const; cv::Mat getMap(float & xMin, float & yMin) const;
cv::Mat getProbMap(float & xMin, float & yMin) const; cv::Mat getProbMap(float & xMin, float & yMin) const;
const pcl::PointCloud<pcl::PointXYZRGB>::Ptr & getMapGround() const {return assembledGround_;} const pcl::PointCloud<pcl::PointXYZRGB>::Ptr & getMapGround() const {return assembledGround_;}

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

@@ -185,7 +185,7 @@ public:
const cv::Mat & obstacles, const cv::Mat & obstacles,
const cv::Mat & empty, const cv::Mat & empty,
const cv::Point3f & viewPoint); const cv::Point3f & viewPoint);
void update(const std::map<int, Transform> & poses); bool update(const std::map<int, Transform> & poses); // return true if map has changed
const RtabmapColorOcTree * octree() const {return octree_;} const RtabmapColorOcTree * octree() const {return octree_;}

9
corelib/src/OccupancyGrid.cpp
View File

@@ -687,7 +687,7 @@ void OccupancyGrid::addToCache(
uInsert(cache_, std::make_pair(nodeId==0?-1:nodeId, std::make_pair(std::make_pair(ground, obstacles), empty))); uInsert(cache_, std::make_pair(nodeId==0?-1:nodeId, std::make_pair(std::make_pair(ground, obstacles), empty)));
} }
void OccupancyGrid::update(const std::map<int, Transform> & posesIn) bool OccupancyGrid::update(const std::map<int, Transform> & posesIn)
{ {
UTimer timer; UTimer timer;
UDEBUG("Update (poses=%d addedNodes_=%d)", (int)posesIn.size(), (int)addedNodes_.size()); UDEBUG("Update (poses=%d addedNodes_=%d)", (int)posesIn.size(), (int)addedNodes_.size());
@@ -904,6 +904,8 @@ void OccupancyGrid::update(const std::map<int, Transform> & posesIn)
poses.push_back(std::make_pair(-1, posesIn.at(0))); poses.push_back(std::make_pair(-1, posesIn.at(0)));
} }
if(!poses.empty())
{
for(std::list<std::pair<int, Transform> >::const_iterator iter = poses.begin(); iter!=poses.end(); ++iter) for(std::list<std::pair<int, Transform> >::const_iterator iter = poses.begin(); iter!=poses.end(); ++iter)
{ {
UASSERT(!iter->second.isNull()); UASSERT(!iter->second.isNull());
@@ -1503,6 +1505,7 @@ void OccupancyGrid::update(const std::map<int, Transform> & posesIn)
assembledEmptyCells_ = util3d::voxelize(assembledEmptyCells_, cellSize_); assembledEmptyCells_ = util3d::voxelize(assembledEmptyCells_, cellSize_);
} }
} }
}
if(!fullUpdate_ && !cloudAssembling_) if(!fullUpdate_ && !cloudAssembling_)
{ {
@@ -1524,7 +1527,9 @@ void OccupancyGrid::update(const std::map<int, Transform> & posesIn)
} }
} }
UDEBUG("Occupancy Grid update time = %f s", timer.ticks()); bool updated = !poses.empty() || graphOptimized || graphChanged;
UDEBUG("Occupancy Grid update time = %f s (updated=%s)", timer.ticks(), updated?"true":"false");
return updated;
} }
} }

7
corelib/src/OctoMap.cpp
View File

@@ -378,7 +378,7 @@ void OctoMap::addToCache(int nodeId,
uInsert(cacheViewPoints_, std::make_pair(nodeId==0?-1:nodeId, viewPoint)); uInsert(cacheViewPoints_, std::make_pair(nodeId==0?-1:nodeId, viewPoint));
} }
void OctoMap::update(const std::map<int, Transform> & poses) bool OctoMap::update(const std::map<int, Transform> & poses)
{ {
UDEBUG("Update (poses=%d addedNodes_=%d)", (int)poses.size(), (int)addedNodes_.size()); UDEBUG("Update (poses=%d addedNodes_=%d)", (int)poses.size(), (int)addedNodes_.size());
@@ -543,6 +543,9 @@ void OctoMap::update(const std::map<int, Transform> & poses)
} }
UDEBUG("orderedPoses = %d", (int)orderedPoses.size()); UDEBUG("orderedPoses = %d", (int)orderedPoses.size());
if(!orderedPoses.empty())
{
float rangeMaxSqrd = rangeMax_*rangeMax_; float rangeMaxSqrd = rangeMax_*rangeMax_;
float cellSize = octree_->getResolution(); float cellSize = octree_->getResolution();
for(std::list<std::pair<int, Transform> >::const_iterator iter=orderedPoses.begin(); iter!=orderedPoses.end(); ++iter) for(std::list<std::pair<int, Transform> >::const_iterator iter=orderedPoses.begin(); iter!=orderedPoses.end(); ++iter)
@@ -844,6 +847,7 @@ void OctoMap::update(const std::map<int, Transform> & poses)
UDEBUG("Did not find %d in cache", iter->first); UDEBUG("Did not find %d in cache", iter->first);
} }
} }
}
if(!fullUpdate_) if(!fullUpdate_)
{ {
@@ -851,6 +855,7 @@ void OctoMap::update(const std::map<int, Transform> & poses)
cacheClouds_.clear(); cacheClouds_.clear();
cacheViewPoints_.clear(); cacheViewPoints_.clear();
} }
return !orderedPoses.empty() || graphOptimized || graphChanged;
} }
void OctoMap::updateMinMax(const octomap::point3d & point) void OctoMap::updateMinMax(const octomap::point3d & point)