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

Parameters: added Grid/FullUpdate (default true). Updated how occupancy grid is updated after loop closure. OctoMap: added tree depth argument when creating 2d map, added full update argument on constructor (default false). MainWindow: using OccupancyGrid object instead of keeping in cache local grids (we can have actual time to update the global grid).

Browse Source
This commit is contained in:
matlabbe
2017-04-07 18:31:11 -04:00
parent 6bfce63060
commit b7dbf27931
13 changed files with 831 additions and 676 deletions
Download Patch File Download Diff File Expand all files Collapse all files

315
corelib/src/OccupancyGrid.cpp
View File

@@ -65,6 +65,7 @@ OccupancyGrid::OccupancyGrid(const ParametersMap & parameters) :
scan2dUnknownSpaceFilled_(Parameters::defaultGridScan2dUnknownSpaceFilled()),
scan2dMaxUnknownSpaceFilledRange_(Parameters::defaultGridScan2dMaxFilledRange()),
projRayTracing_(Parameters::defaultGridProjRayTracing()),
fullUpdate_(Parameters::defaultGridFullUpdate()),
xMin_(0.0f),
yMin_(0.0f)
{
@@ -131,6 +132,7 @@ void OccupancyGrid::parseParameters(const ParametersMap & parameters)
Parameters::parse(parameters, Parameters::kGridScan2dUnknownSpaceFilled(), scan2dUnknownSpaceFilled_);
Parameters::parse(parameters, Parameters::kGridScan2dMaxFilledRange(), scan2dMaxUnknownSpaceFilledRange_);
Parameters::parse(parameters, Parameters::kGridProjRayTracing(), projRayTracing_);
Parameters::parse(parameters, Parameters::kGridFullUpdate(), fullUpdate_);
// convert ROI from string to vector
ParametersMap::const_iterator iter;
@@ -383,7 +385,7 @@ void OccupancyGrid::addToCache(
const cv::Mat & obstacles)
{
UDEBUG("nodeId=%d", nodeId);
cache_.insert(std::make_pair(nodeId, std::make_pair(ground, obstacles)));
uInsert(cache_, std::make_pair(nodeId, std::make_pair(ground, obstacles)));
}
void OccupancyGrid::update(const std::map<int, Transform> & posesIn, float minMapSize, float footprintRadius)
@@ -391,7 +393,7 @@ void OccupancyGrid::update(const std::map<int, Transform> & posesIn, float minMa
UTimer timer;
UDEBUG("Update (poses=%d addedNodes_=%d)", (int)posesIn.size(), (int)addedNodes_.size());
float margin = cellSize_*10.0f+footprintRadius;
float margin = cellSize_*10.0f+(footprintRadius>cellSize_*1.5f?float(int(footprintRadius/cellSize_)+1):0.0f)*cellSize_;
float minX=-minMapSize/2.0f;
float minY=-minMapSize/2.0f;
@@ -401,7 +403,7 @@ void OccupancyGrid::update(const std::map<int, Transform> & posesIn, float minMa
std::map<int, cv::Mat> emptyLocalMaps;
std::map<int, cv::Mat> occupiedLocalMaps;
// First, check of the graph has changed. If so, re-create the octree by moving all occupied nodes.
// First, check of the graph has changed. If so, re-create the map by moving all occupied nodes.
bool graphChanged = false;
std::map<int, Transform> transforms;
for(std::map<int, Transform>::iterator iter=addedNodes_.begin(); iter!=addedNodes_.end(); ++iter)
@@ -448,75 +450,78 @@ void OccupancyGrid::update(const std::map<int, Transform> & posesIn, float minMa
if(graphChanged && !map_.empty())
{
UINFO("Graph changed!");
// 1) recreate all local maps
UASSERT(map_.cols == mapInfo_.cols &&
map_.rows == mapInfo_.rows);
std::map<int, std::pair<int, int> > tmpIndices;
for(std::map<int, std::pair<int, int> >::iterator iter=cellCount_.begin(); iter!=cellCount_.end(); ++iter)
if(!fullUpdate_)
{
if(iter->second.first)
// 1) recreate all local maps
UASSERT(map_.cols == mapInfo_.cols &&
map_.rows == mapInfo_.rows);
std::map<int, std::pair<int, int> > tmpIndices;
for(std::map<int, std::pair<int, int> >::iterator iter=cellCount_.begin(); iter!=cellCount_.end(); ++iter)
{
emptyLocalMaps.insert(std::make_pair( iter->first, cv::Mat(1, iter->second.first, CV_32FC2)));
}
if(iter->second.second)
{
occupiedLocalMaps.insert(std::make_pair( iter->first, cv::Mat(1, iter->second.second, CV_32FC2)));
}
tmpIndices.insert(std::make_pair(iter->first, std::make_pair(0,0)));
}
for(int y=1; y<map_.rows-1; ++y)
{
for(int x=1; x<map_.cols-1; ++x)
{
float * info = mapInfo_.ptr<float>(y,x);
int nodeId = (int)info[0];
if(nodeId > 0 && map_.at<char>(y,x) >= 0)
if(iter->second.first)
{
std::map<int, Transform>::iterator tter = transforms.find(nodeId);
if(tter != transforms.end() && !uContains(cache_, nodeId))
emptyLocalMaps.insert(std::make_pair( iter->first, cv::Mat(1, iter->second.first, CV_32FC2)));
}
if(iter->second.second)
{
occupiedLocalMaps.insert(std::make_pair( iter->first, cv::Mat(1, iter->second.second, CV_32FC2)));
}
tmpIndices.insert(std::make_pair(iter->first, std::make_pair(0,0)));
}
for(int y=1; y<map_.rows-1; ++y)
{
for(int x=1; x<map_.cols-1; ++x)
{
float * info = mapInfo_.ptr<float>(y,x);
int nodeId = (int)info[0];
if(nodeId > 0 && map_.at<char>(y,x) >= 0)
{
cv::Point3f pt(info[1], info[2], 0.0f);
pt = util3d::transformPoint(pt, tter->second);
if(minX > pt.x)
minX = pt.x;
else if(maxX < pt.x)
maxX = pt.x;
if(minY > pt.y)
minY = pt.y;
else if(maxY < pt.y)
maxY = pt.y;
std::map<int, std::pair<int, int> >::iterator jter = tmpIndices.find(nodeId);
if(map_.at<char>(y, x) == 0)
std::map<int, Transform>::iterator tter = transforms.find(nodeId);
if(tter != transforms.end() && !uContains(cache_, nodeId))
{
// ground
std::map<int, cv::Mat>::iterator iter = emptyLocalMaps.find(nodeId);
UASSERT(iter != emptyLocalMaps.end());
UASSERT(jter->second.first < iter->second.cols);
float * ptf = iter->second.ptr<float>(0,jter->second.first++);
ptf[0] = pt.x;
ptf[1] = pt.y;
}
else
{
// obstacle
std::map<int, cv::Mat>::iterator iter = occupiedLocalMaps.find(nodeId);
UASSERT(iter != occupiedLocalMaps.end());
UASSERT(iter!=occupiedLocalMaps.end());
UASSERT(jter->second.second < iter->second.cols);
float * ptf = iter->second.ptr<float>(0,jter->second.second++);
ptf[0] = pt.x;
ptf[1] = pt.y;
cv::Point3f pt(info[1], info[2], 0.0f);
pt = util3d::transformPoint(pt, tter->second);
if(minX > pt.x)
minX = pt.x;
else if(maxX < pt.x)
maxX = pt.x;
if(minY > pt.y)
minY = pt.y;
else if(maxY < pt.y)
maxY = pt.y;
std::map<int, std::pair<int, int> >::iterator jter = tmpIndices.find(nodeId);
if(map_.at<char>(y, x) == 0)
{
// ground
std::map<int, cv::Mat>::iterator iter = emptyLocalMaps.find(nodeId);
UASSERT(iter != emptyLocalMaps.end());
UASSERT(jter->second.first < iter->second.cols);
float * ptf = iter->second.ptr<float>(0,jter->second.first++);
ptf[0] = pt.x;
ptf[1] = pt.y;
}
else
{
// obstacle
std::map<int, cv::Mat>::iterator iter = occupiedLocalMaps.find(nodeId);
UASSERT(iter != occupiedLocalMaps.end());
UASSERT(iter!=occupiedLocalMaps.end());
UASSERT(jter->second.second < iter->second.cols);
float * ptf = iter->second.ptr<float>(0,jter->second.second++);
ptf[0] = pt.x;
ptf[1] = pt.y;
}
}
}
}
}
UDEBUG("min (%f,%f) max(%f,%f)", minX, minY, maxX, maxY);
}
UDEBUG("min (%f,%f) max(%f,%f)", minX, minY, maxX, maxY);
addedNodes_.clear();
map_ = cv::Mat();
mapInfo_ = cv::Mat();
@@ -527,25 +532,36 @@ void OccupancyGrid::update(const std::map<int, Transform> & posesIn, float minMa
else if(!map_.empty())
{
// update
minX=xMin_+margin;
minY=yMin_+margin;
minX=xMin_+margin+cellSize_/2.0f;
minY=yMin_+margin+cellSize_/2.0f;
maxX=xMin_+float(map_.cols)*cellSize_ - margin;
maxY=yMin_+float(map_.rows)*cellSize_ - margin;
undefinedSize = false;
}
bool incrementalGraphUpdate = graphChanged && !fullUpdate_;
std::list<std::pair<int, Transform> > poses;
// place negative poses at the end
for(std::map<int, Transform>::const_reverse_iterator iter = posesIn.rbegin(); iter!=posesIn.rend(); ++iter)
int lastId = addedNodes_.size()?addedNodes_.rbegin()->first:0;
UDEBUG("Last id = %d", lastId);
if(lastId >= 0)
{
if(iter->first>0)
{
poses.push_front(*iter);
}
else
for(std::map<int, Transform>::const_iterator iter=posesIn.upper_bound(lastId); iter!=posesIn.end(); ++iter)
{
poses.push_back(*iter);
}
// insert negative after
for(std::map<int, Transform>::const_iterator iter=posesIn.begin(); iter!=posesIn.end(); ++iter)
{
if(iter->first < 0)
{
poses.push_back(*iter);
}
else
{
break;
}
}
}
for(std::list<std::pair<int, Transform> >::const_iterator iter = poses.begin(); iter!=poses.end(); ++iter)
{
@@ -662,9 +678,12 @@ void OccupancyGrid::update(const std::map<int, Transform> & posesIn, float minMa
{
//Get map size
float xMin = minX-margin;
xMin -= cellSize_/2.0f;
float yMin = minY-margin;
yMin -= cellSize_/2.0f;
float xMax = maxX+margin;
float yMax = maxY+margin;
if(fabs((yMax - yMin) / cellSize_) > 99999 ||
fabs((xMax - xMin) / cellSize_) > 99999)
{
@@ -675,7 +694,7 @@ void OccupancyGrid::update(const std::map<int, Transform> & posesIn, float minMa
else
{
UDEBUG("map min=(%f, %f) odlMin(%f,%f) max=(%f,%f)", xMin, yMin, xMin_, yMin_, xMax, yMax);
cv::Size newMapSize((xMax - xMin) / cellSize_ + 0.5f, (yMax - yMin) / cellSize_ + 0.5f);
cv::Size newMapSize((xMax - xMin) / cellSize_+0.5f, (yMax - yMin) / cellSize_+0.5f);
if(map_.empty())
{
UDEBUG("Map empty!");
@@ -695,6 +714,11 @@ void OccupancyGrid::update(const std::map<int, Transform> & posesIn, float minMa
}
else
{
UASSERT(xMin <= xMin_);
UASSERT(yMin <= yMin_);
UASSERT(xMax >= xMin_+float(map_.cols)*cellSize_);
UASSERT(yMax >= yMin_+float(map_.rows)*cellSize_);
UDEBUG("Copy map");
// copy the old map in the new map
// make sure the translation is cellSize
@@ -711,8 +735,10 @@ void OccupancyGrid::update(const std::map<int, Transform> & posesIn, float minMa
yMin = yMin_-float(deltaY)*cellSize_;
}
UDEBUG("deltaX=%d, deltaY=%d", deltaX, deltaY);
newMapSize.width = (xMax - xMin) / cellSize_ + 0.5f;
newMapSize.height = (yMax - yMin) / cellSize_ + 0.5f;
newMapSize.width = (xMax - xMin) / cellSize_+0.5f;
newMapSize.height = (yMax - yMin) / cellSize_+0.5f;
UDEBUG("%d/%d -> %d/%d", map_.cols, map_.rows, newMapSize.width, newMapSize.height);
UASSERT(newMapSize.width >= map_.cols && newMapSize.height >= map_.rows);
map = cv::Mat::ones(newMapSize, CV_8S)*-1;
mapInfo = cv::Mat::zeros(newMapSize, mapInfo_.type());
map_.copyTo(map(cv::Rect(deltaX, deltaY, map_.cols, map_.rows)));
@@ -743,12 +769,12 @@ void OccupancyGrid::update(const std::map<int, Transform> & posesIn, float minMa
for(int i=0; i<iter->second.cols; ++i)
{
float * ptf = iter->second.ptr<float>(0,i);
cv::Point2i pt((ptf[0]-xMin)/cellSize_ + 0.5f, (ptf[1]-yMin)/cellSize_ + 0.5f);
cv::Point2i pt((ptf[0]-xMin)/cellSize_, (ptf[1]-yMin)/cellSize_);
UASSERT_MSG(pt.y < map.rows && pt.x < map.cols,
uFormat("%d: pt=(%d,%d) map=%dx%d rawPt=(%f,%f) xMin=%f yMin=%f channels=%dvs%d",
kter->first, pt.x, pt.y, map.cols, map.rows, ptf[0], ptf[1], xMin, yMin, iter->second.channels(), mapInfo.channels()-1).c_str());
char & value = map.at<char>(pt.y, pt.x);
if(value != -2)
if(value != -2 && (!incrementalGraphUpdate || value==-1))
{
float * info = mapInfo.ptr<float>(pt.y, pt.x);
int nodeId = (int)info[0];
@@ -792,8 +818,8 @@ void OccupancyGrid::update(const std::map<int, Transform> & posesIn, float minMa
if(footprintRadius >= cellSize_*1.5f)
{
// place free space under the footprint of the robot
cv::Point2i ptBegin((kter->second.x()-footprintRadius-xMin)/cellSize_ + 0.5f, (kter->second.y()-footprintRadius-yMin)/cellSize_ + 0.5f);
cv::Point2i ptEnd((kter->second.x()+footprintRadius-xMin)/cellSize_ + 0.5f, (kter->second.y()+footprintRadius-yMin)/cellSize_ + 0.5f);
cv::Point2i ptBegin((kter->second.x()-footprintRadius-xMin)/cellSize_, (kter->second.y()-footprintRadius-yMin)/cellSize_);
cv::Point2i ptEnd((kter->second.x()+footprintRadius-xMin)/cellSize_, (kter->second.y()+footprintRadius-yMin)/cellSize_);
if(ptBegin.x < 0)
ptBegin.x = 0;
if(ptEnd.x >= map.cols)
@@ -839,8 +865,8 @@ void OccupancyGrid::update(const std::map<int, Transform> & posesIn, float minMa
if(kter->first > 0)
{
info[0] = (float)kter->first;
info[1] = float(i) * cellSize_ + xMin_ + 0.5f;
info[2] = float(j) * cellSize_ + yMin_ + 0.5f;
info[1] = float(i) * cellSize_ + xMin;
info[2] = float(j) * cellSize_ + yMin;
cter->second.first+=1;
}
value = -2; // free space (footprint)
@@ -853,7 +879,7 @@ void OccupancyGrid::update(const std::map<int, Transform> & posesIn, float minMa
for(int i=0; i<jter->second.cols; ++i)
{
float * ptf = jter->second.ptr<float>(0,i);
cv::Point2i pt((ptf[0]-xMin)/cellSize_ + 0.5f, (ptf[1]-yMin)/cellSize_ + 0.5f);
cv::Point2i pt((ptf[0]-xMin)/cellSize_, (ptf[1]-yMin)/cellSize_);
UASSERT_MSG(pt.y < map.rows && pt.x < map.cols,
uFormat("%d: pt=(%d,%d) map=%dx%d rawPt=(%f,%f) xMin=%f yMin=%f channels=%dvs%d",
kter->first, pt.x, pt.y, map.cols, map.rows, ptf[0], ptf[1], xMin, yMin, jter->second.channels(), mapInfo.channels()-1).c_str());
@@ -900,36 +926,110 @@ void OccupancyGrid::update(const std::map<int, Transform> & posesIn, float minMa
}
}
// fill holes and put footprint values to empty (0)
//pcl::PointCloud<pcl::PointXYZ>::Ptr cloud(new pcl::PointCloud<pcl::PointXYZ>);
//cloud->resize(map.rows*map.cols);
//int oi=0;
for(int i=1; i<map.rows-1; ++i)
if(footprintRadius >= cellSize_*1.5f || incrementalGraphUpdate)
{
for(int j=1; j<map.cols-1; ++j)
for(int i=1; i<map.rows-1; ++i)
{
char & value = map.at<char>(i, j);
if(value == -2)
for(int j=1; j<map.cols-1; ++j)
{
value = 0;
}
char & value = map.at<char>(i, j);
if(value == -2)
{
value = 0;
}
char sum = (map.at<char>(i+1, j) != -1?1:0) +
(map.at<char>(i-1, j) != -1?1:0) +
(map.at<char>(i, j+1) != -1?1:0) +
(map.at<char>(i, j-1) != -1?1:0);
if(value == -1 && sum >=3)
{
value = 0;
}
if(incrementalGraphUpdate && value == -1)
{
float * info = mapInfo.ptr<float>(i, j);
//float * info = mapInfo.ptr<float>(i,j);
//if(info[0] > 0)
//{
// cloud->at(oi).x = info[1];
// cloud->at(oi).y = info[2];
// oi++;
//}
// fill obstacle
if(map.at<char>(i+1, j) == 100 && map.at<char>(i-1, j) == 100)
{
value = 100;
// associate with the nearest pose
if(mapInfo.ptr<float>(i+1, j)[0]>0.0f)
{
info[0] = mapInfo.ptr<float>(i+1, j)[0];
info[1] = float(j) * cellSize_ + xMin;
info[2] = float(i) * cellSize_ + yMin;
std::map<int, std::pair<int, int> >::iterator cter = cellCount_.find(int(info[0]));
UASSERT(cter!=cellCount_.end());
cter->second.second+=1;
}
else if(mapInfo.ptr<float>(i-1, j)[0]>0.0f)
{
info[0] = mapInfo.ptr<float>(i-1, j)[0];
info[1] = float(j) * cellSize_ + xMin;
info[2] = float(i) * cellSize_ + yMin;
std::map<int, std::pair<int, int> >::iterator cter = cellCount_.find(int(info[0]));
UASSERT(cter!=cellCount_.end());
cter->second.second+=1;
}
}
else if(map.at<char>(i, j+1) == 100 && map.at<char>(i, j-1) == 100)
{
value = 100;
// associate with the nearest pose
if(mapInfo.ptr<float>(i, j+1)[0]>0.0f)
{
info[0] = mapInfo.ptr<float>(i, j+1)[0];
info[1] = float(j) * cellSize_ + xMin;
info[2] = float(i) * cellSize_ + yMin;
std::map<int, std::pair<int, int> >::iterator cter = cellCount_.find(int(info[0]));
UASSERT(cter!=cellCount_.end());
cter->second.second+=1;
}
else if(mapInfo.ptr<float>(i, j-1)[0]>0.0f)
{
info[0] = mapInfo.ptr<float>(i, j-1)[0];
info[1] = float(j) * cellSize_ + xMin;
info[2] = float(i) * cellSize_ + yMin;
std::map<int, std::pair<int, int> >::iterator cter = cellCount_.find(int(info[0]));
UASSERT(cter!=cellCount_.end());
cter->second.second+=1;
}
}
else
{
// fill empty
char sum = (map.at<char>(i+1, j) == 0?1:0) +
(map.at<char>(i-1, j) == 0?1:0) +
(map.at<char>(i, j+1) == 0?1:0) +
(map.at<char>(i, j-1) == 0?1:0);
if(sum >=3)
{
value = 0;
// associate with the nearest pose, only check two cases (as 3 are required)
if(map.at<char>(i+1, j) != -1 && mapInfo.ptr<float>(i+1, j)[0]>0.0f)
{
info[0] = mapInfo.ptr<float>(i+1, j)[0];
info[1] = float(j) * cellSize_ + xMin;
info[2] = float(i) * cellSize_ + yMin;
std::map<int, std::pair<int, int> >::iterator cter = cellCount_.find(int(info[0]));
UASSERT(cter!=cellCount_.end());
cter->second.first+=1;
}
else if(map.at<char>(i-1, j) != -1 && mapInfo.ptr<float>(i-1, j)[0]>0.0f)
{
info[0] = mapInfo.ptr<float>(i-1, j)[0];
info[1] = float(j) * cellSize_ + xMin;
info[2] = float(i) * cellSize_ + yMin;
std::map<int, std::pair<int, int> >::iterator cter = cellCount_.find(int(info[0]));
UASSERT(cter!=cellCount_.end());
cter->second.first+=1;
}
}
}
}
//float * info = mapInfo.ptr<float>(i,j);
//if(info[0] > 0)
//{
// cloud->at(oi).x = info[1];
// cloud->at(oi).y = info[2];
// oi++;
//}
}
}
}
//if(graphChanged)
@@ -960,7 +1060,10 @@ void OccupancyGrid::update(const std::map<int, Transform> & posesIn, float minMa
}
}
cache_.clear();
if(!fullUpdate_)
{
cache_.clear();
}
UDEBUG("Occupancy Grid update time = %f s", timer.ticks());
}

445
corelib/src/OctoMap.cpp
View File

@@ -35,9 +35,10 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
namespace rtabmap {
OctoMap::OctoMap(float voxelSize, float occupancyThr) :
OctoMap::OctoMap(float voxelSize, float occupancyThr, bool fullUpdate) :
octree_(new octomap::ColorOcTree(voxelSize)),
hasColor_(false)
hasColor_(false),
fullUpdate_(fullUpdate)
{
octree_->setOccupancyThres(occupancyThr);
UASSERT(voxelSize>0.0f);
@@ -67,8 +68,8 @@ void OctoMap::addToCache(int nodeId,
const pcl::PointXYZ & viewPoint)
{
UDEBUG("nodeId=%d", nodeId);
cacheClouds_.insert(std::make_pair(nodeId, std::make_pair(ground, obstacles)));
cacheViewPoints_.insert(std::make_pair(nodeId, cv::Point3f(viewPoint.x, viewPoint.y, viewPoint.z)));
uInsert(cacheClouds_, std::make_pair(nodeId, std::make_pair(ground, obstacles)));
uInsert(cacheViewPoints_, std::make_pair(nodeId, cv::Point3f(viewPoint.x, viewPoint.y, viewPoint.z)));
}
void OctoMap::addToCache(int nodeId,
const cv::Mat & ground,
@@ -78,8 +79,8 @@ void OctoMap::addToCache(int nodeId,
UASSERT(ground.empty() || ground.type() == CV_32FC3 || ground.type() == CV_32FC(4) || ground.type() == CV_32FC(6));
UASSERT(obstacles.empty() || obstacles.type() == CV_32FC3 || obstacles.type() == CV_32FC(4) || obstacles.type() == CV_32FC(6));
UDEBUG("nodeId=%d", nodeId);
cache_.insert(std::make_pair(nodeId, std::make_pair(ground, obstacles)));
cacheViewPoints_.insert(std::make_pair(nodeId, viewPoint));
uInsert(cache_, std::make_pair(nodeId, std::make_pair(ground, obstacles)));
uInsert(cacheViewPoints_, std::make_pair(nodeId, viewPoint));
}
void OctoMap::update(const std::map<int, Transform> & poses)
@@ -113,65 +114,77 @@ void OctoMap::update(const std::map<int, Transform> & poses)
if(graphChanged)
{
UINFO("Graph changed!");
octomap::ColorOcTree * newOcTree = new octomap::ColorOcTree(octree_->getResolution());
std::map<octomap::ColorOcTreeNode*, OcTreeNodeInfo > newOccupiedCells;
int copied=0;
for(std::map<octomap::ColorOcTreeNode*, OcTreeNodeInfo >::iterator iter = occupiedCells_.begin();
iter!=occupiedCells_.end();
++iter)
if(fullUpdate_)
{
std::map<int, Transform>::iterator jter = transforms.find(iter->second.nodeRefId_);
if(jter != transforms.end())
// clear all but keep cache
octree_->clear();
occupiedCells_.clear();
addedNodes_.clear();
keyRay_ = octomap::KeyRay();
hasColor_ = false;
}
else
{
octomap::ColorOcTree * newOcTree = new octomap::ColorOcTree(octree_->getResolution());
std::map<octomap::ColorOcTreeNode*, OcTreeNodeInfo > newOccupiedCells;
int copied=0;
for(std::map<octomap::ColorOcTreeNode*, OcTreeNodeInfo >::iterator iter = occupiedCells_.begin();
iter!=occupiedCells_.end();
++iter)
{
octomap::point3d pt = octree_->keyToCoord(iter->second.key_);
std::map<int, Transform>::iterator pter = addedNodes_.find(iter->second.nodeRefId_);
UASSERT(pter != addedNodes_.end());
cv::Point3f cvPt(pt.x(), pt.y(), pt.z());
cvPt = util3d::transformPoint(cvPt, jter->second);
octomap::OcTreeKey key;
if(newOcTree->coordToKeyChecked(cvPt.x, cvPt.y, cvPt.z, key))
std::map<int, Transform>::iterator jter = transforms.find(iter->second.nodeRefId_);
if(jter != transforms.end())
{
octomap::ColorOcTreeNode * n = newOcTree->updateNode(key, iter->second.isObstacle_);
if(n)
octomap::point3d pt = octree_->keyToCoord(iter->second.key_);
std::map<int, Transform>::iterator pter = addedNodes_.find(iter->second.nodeRefId_);
UASSERT(pter != addedNodes_.end());
cv::Point3f cvPt(pt.x(), pt.y(), pt.z());
cvPt = util3d::transformPoint(cvPt, jter->second);
octomap::OcTreeKey key;
if(newOcTree->coordToKeyChecked(cvPt.x, cvPt.y, cvPt.z, key))
{
++copied;
uInsert(newOccupiedCells, std::make_pair(n, OcTreeNodeInfo(jter->first, key, iter->second.isObstacle_)));
newOcTree->setNodeColor(key, iter->first->getColor().r, iter->first->getColor().g, iter->first->getColor().b);
octomap::ColorOcTreeNode * n = newOcTree->updateNode(key, iter->second.isObstacle_);
if(n)
{
++copied;
uInsert(newOccupiedCells, std::make_pair(n, OcTreeNodeInfo(jter->first, key, iter->second.isObstacle_)));
newOcTree->setNodeColor(key, iter->first->getColor().r, iter->first->getColor().g, iter->first->getColor().b);
}
else
{
UERROR("Could not update node at (%f,%f,%f)", cvPt.x, cvPt.y, cvPt.z);
}
}
else
{
UERROR("Could not update node at (%f,%f,%f)", cvPt.x, cvPt.y, cvPt.z);
UERROR("Could not find key for (%f,%f,%f)", cvPt.x, cvPt.y, cvPt.z);
}
}
else
else if(jter == transforms.end() && iter->second.nodeRefId_ > 0)
{
UERROR("Could not find key for (%f,%f,%f)", cvPt.x, cvPt.y, cvPt.z);
UWARN("Could not find a transform for point linked to node %d (transforms=%d)", iter->second.nodeRefId_, (int)transforms.size());
}
}
else if(jter == transforms.end() && iter->second.nodeRefId_ > 0)
{
UWARN("Could not find a transform for point linked to node %d (transforms=%d)", iter->second.nodeRefId_, (int)transforms.size());
}
}
UDEBUG("%d/%d", copied, (int)occupiedCells_.size());
delete octree_;
octree_ = newOcTree;
occupiedCells_ = newOccupiedCells;
UDEBUG("%d/%d", copied, (int)occupiedCells_.size());
delete octree_;
octree_ = newOcTree;
occupiedCells_ = newOccupiedCells;
//update added poses
addedNodes_ = updatedAddedNodes;
//update added poses
addedNodes_ = updatedAddedNodes;
}
}
// Original version from A. Hornung:
// https://github.com/OctoMap/octomap_mapping/blob/jade-devel/octomap_server/src/OctomapServer.cpp#L356
//
std::list<std::pair<int, Transform> > orderedPoses;
int lastId = addedNodes_.size()?addedNodes_.rbegin()->first:0;
UDEBUG("Last id = %d", lastId);
if(lastId >= 0)
{
std::list<std::pair<int, Transform> > orderedPoses;
for(std::map<int, Transform>::const_iterator iter=poses.upper_bound(lastId); iter!=poses.end(); ++iter)
{
orderedPoses.push_back(*iter);
@@ -188,173 +201,176 @@ void OctoMap::update(const std::map<int, Transform> & poses)
break;
}
}
}
UDEBUG("orderedPoses = %d", (int)orderedPoses.size());
for(std::list<std::pair<int, Transform> >::const_iterator iter=orderedPoses.begin(); iter!=orderedPoses.end(); ++iter)
UDEBUG("orderedPoses = %d", (int)orderedPoses.size());
for(std::list<std::pair<int, Transform> >::const_iterator iter=orderedPoses.begin(); iter!=orderedPoses.end(); ++iter)
{
std::map<int, std::pair<pcl::PointCloud<pcl::PointXYZRGB>::Ptr, pcl::PointCloud<pcl::PointXYZRGB>::Ptr> >::iterator cloudIter;
std::map<int, std::pair<cv::Mat, cv::Mat> >::iterator occupancyIter;
std::map<int, cv::Point3f>::iterator viewPointIter;
cloudIter = cacheClouds_.find(iter->first);
occupancyIter = cache_.find(iter->first);
viewPointIter = cacheViewPoints_.find(iter->first);
if(occupancyIter != cache_.end() || cloudIter != cacheClouds_.end())
{
std::map<int, std::pair<pcl::PointCloud<pcl::PointXYZRGB>::Ptr, pcl::PointCloud<pcl::PointXYZRGB>::Ptr> >::iterator cloudIter;
std::map<int, std::pair<cv::Mat, cv::Mat> >::iterator occupancyIter;
std::map<int, cv::Point3f>::iterator viewPointIter;
cloudIter = cacheClouds_.find(iter->first);
occupancyIter = cache_.find(iter->first);
viewPointIter = cacheViewPoints_.find(iter->first);
if(occupancyIter != cache_.end() || cloudIter != cacheClouds_.end())
UDEBUG("Adding %d to octomap (resolution=%f)", iter->first, octree_->getResolution());
UASSERT(viewPointIter != cacheViewPoints_.end());
octomap::point3d sensorOrigin(iter->second.x(), iter->second.y(), iter->second.z());
sensorOrigin += octomap::point3d(viewPointIter->second.x, viewPointIter->second.y, viewPointIter->second.z);
octomap::OcTreeKey tmpKey;
if (!octree_->coordToKeyChecked(sensorOrigin, tmpKey)
|| !octree_->coordToKeyChecked(sensorOrigin, tmpKey))
{
UDEBUG("Adding %d to octomap (resolution=%f)", iter->first, octree_->getResolution());
UASSERT(viewPointIter != cacheViewPoints_.end());
octomap::point3d sensorOrigin(iter->second.x(), iter->second.y(), iter->second.z());
sensorOrigin += octomap::point3d(viewPointIter->second.x, viewPointIter->second.y, viewPointIter->second.z);
octomap::OcTreeKey tmpKey;
if (!octree_->coordToKeyChecked(sensorOrigin, tmpKey)
|| !octree_->coordToKeyChecked(sensorOrigin, tmpKey))
{
UERROR("Could not generate Key for origin ", sensorOrigin.x(), sensorOrigin.y(), sensorOrigin.z());
}
// instead of direct scan insertion, compute update to filter ground:
octomap::KeySet free_cells, occupied_cells, ground_cells;
// insert ground points only as free:
unsigned int maxGroundPts = occupancyIter != cache_.end()?occupancyIter->second.first.cols:cloudIter->second.first->size();
UDEBUG("%d: compute free cells (from %d ground points)", iter->first, (int)maxGroundPts);
Eigen::Affine3f t = iter->second.toEigen3f();
for (unsigned int i=0; i<maxGroundPts; ++i)
{
pcl::PointXYZRGB pt;
if(occupancyIter != cache_.end())
{
pt = util3d::laserScanToPointRGB(occupancyIter->second.first, i);
pt = pcl::transformPoint(pt, t);
}
else
{
pt = pcl::transformPoint(cloudIter->second.first->at(i), t);
}
octomap::point3d point(pt.x, pt.y, pt.z);
// only clear space (ground points)
if (octree_->computeRayKeys(sensorOrigin, point, keyRay_))
{
free_cells.insert(keyRay_.begin(), keyRay_.end());
}
// occupied endpoint
octomap::OcTreeKey key;
if (octree_->coordToKeyChecked(point, key))
{
ground_cells.insert(key);
octomap::ColorOcTreeNode * n = octree_->updateNode(key, false);
if(n)
{
if(!hasColor_ && (pt.r !=0 || pt.g != 0 || pt.b != 0))
{
hasColor_ = true;
}
octree_->averageNodeColor(key, pt.r, pt.g, pt.b);
if(iter->first > 0)
{
uInsert(occupiedCells_, std::make_pair(n, OcTreeNodeInfo(iter->first, key, false)));
}
else
{
occupiedCells_.insert(std::make_pair(n, OcTreeNodeInfo(iter->first, key, false)));
}
}
}
}
UDEBUG("%d: free cells = %d", iter->first, (int)free_cells.size());
// all other points: free on ray, occupied on endpoint:
unsigned int maxObstaclePts = occupancyIter != cache_.end()?occupancyIter->second.second.cols:cloudIter->second.second->size();
UDEBUG("%d: compute occupied cells (from %d obstacle points)", iter->first, (int)maxObstaclePts);
for (unsigned int i=0; i<maxObstaclePts; ++i)
{
pcl::PointXYZRGB pt;
if(occupancyIter != cache_.end())
{
pt = util3d::laserScanToPointRGB(occupancyIter->second.second, i);
pt = pcl::transformPoint(pt, t);
}
else
{
pt = pcl::transformPoint(cloudIter->second.second->at(i), t);
}
octomap::point3d point(pt.x, pt.y, pt.z);
// free cells
if (octree_->computeRayKeys(sensorOrigin, point, keyRay_))
{
free_cells.insert(keyRay_.begin(), keyRay_.end());
}
// occupied endpoint
octomap::OcTreeKey key;
if (octree_->coordToKeyChecked(point, key))
{
occupied_cells.insert(key);
octomap::ColorOcTreeNode * n = octree_->updateNode(key, true);
if(n)
{
if(!hasColor_ && (pt.r !=0 || pt.g != 0 || pt.b != 0))
{
hasColor_ = true;
}
octree_->averageNodeColor(key, pt.r, pt.g, pt.b);
if(iter->first > 0)
{
uInsert(occupiedCells_, std::make_pair(n, OcTreeNodeInfo(iter->first, key, true)));
}
else
{
occupiedCells_.insert(std::make_pair(n, OcTreeNodeInfo(iter->first, key, true)));
}
}
}
}
UDEBUG("%d: occupied cells=%d free cells=%d", iter->first, (int)occupied_cells.size(), (int)free_cells.size());
// mark free cells only if not seen occupied in this cloud
for(octomap::KeySet::iterator it = free_cells.begin(), end=free_cells.end(); it!= end; ++it)
{
if (occupied_cells.find(*it) == occupied_cells.end() &&
ground_cells.find(*it) == ground_cells.end())
{
octomap::ColorOcTreeNode * n = octree_->updateNode(*it, false);
if(n)
{
std::map<octomap::ColorOcTreeNode*, OcTreeNodeInfo>::iterator gter;
gter = occupiedCells_.find(n);
if(gter != occupiedCells_.end() && gter->second.isObstacle_)
{
occupiedCells_.erase(gter);
}
}
}
}
// compress map
//octree_->prune();
// ignore negative ids as they are temporary clouds
if(iter->first > 0)
{
addedNodes_.insert(*iter);
}
UDEBUG("%d: end", iter->first);
UERROR("Could not generate Key for origin ", sensorOrigin.x(), sensorOrigin.y(), sensorOrigin.z());
}
else
// instead of direct scan insertion, compute update to filter ground:
octomap::KeySet free_cells, occupied_cells, ground_cells;
// insert ground points only as free:
unsigned int maxGroundPts = occupancyIter != cache_.end()?occupancyIter->second.first.cols:cloudIter->second.first->size();
UDEBUG("%d: compute free cells (from %d ground points)", iter->first, (int)maxGroundPts);
Eigen::Affine3f t = iter->second.toEigen3f();
for (unsigned int i=0; i<maxGroundPts; ++i)
{
UDEBUG("Did not find %d in cache", iter->first);
pcl::PointXYZRGB pt;
if(occupancyIter != cache_.end())
{
pt = util3d::laserScanToPointRGB(occupancyIter->second.first, i);
pt = pcl::transformPoint(pt, t);
}
else
{
pt = pcl::transformPoint(cloudIter->second.first->at(i), t);
}
octomap::point3d point(pt.x, pt.y, pt.z);
// only clear space (ground points)
if (octree_->computeRayKeys(sensorOrigin, point, keyRay_))
{
free_cells.insert(keyRay_.begin(), keyRay_.end());
}
// occupied endpoint
octomap::OcTreeKey key;
if (octree_->coordToKeyChecked(point, key))
{
ground_cells.insert(key);
octomap::ColorOcTreeNode * n = octree_->updateNode(key, false);
if(n)
{
if(!hasColor_ && (pt.r !=0 || pt.g != 0 || pt.b != 0))
{
hasColor_ = true;
}
octree_->averageNodeColor(key, pt.r, pt.g, pt.b);
if(iter->first > 0)
{
uInsert(occupiedCells_, std::make_pair(n, OcTreeNodeInfo(iter->first, key, false)));
}
else
{
occupiedCells_.insert(std::make_pair(n, OcTreeNodeInfo(iter->first, key, false)));
}
}
}
}
UDEBUG("%d: free cells = %d", iter->first, (int)free_cells.size());
// all other points: free on ray, occupied on endpoint:
unsigned int maxObstaclePts = occupancyIter != cache_.end()?occupancyIter->second.second.cols:cloudIter->second.second->size();
UDEBUG("%d: compute occupied cells (from %d obstacle points)", iter->first, (int)maxObstaclePts);
for (unsigned int i=0; i<maxObstaclePts; ++i)
{
pcl::PointXYZRGB pt;
if(occupancyIter != cache_.end())
{
pt = util3d::laserScanToPointRGB(occupancyIter->second.second, i);
pt = pcl::transformPoint(pt, t);
}
else
{
pt = pcl::transformPoint(cloudIter->second.second->at(i), t);
}
octomap::point3d point(pt.x, pt.y, pt.z);
// free cells
if (octree_->computeRayKeys(sensorOrigin, point, keyRay_))
{
free_cells.insert(keyRay_.begin(), keyRay_.end());
}
// occupied endpoint
octomap::OcTreeKey key;
if (octree_->coordToKeyChecked(point, key))
{
occupied_cells.insert(key);
octomap::ColorOcTreeNode * n = octree_->updateNode(key, true);
if(n)
{
if(!hasColor_ && (pt.r !=0 || pt.g != 0 || pt.b != 0))
{
hasColor_ = true;
}
octree_->averageNodeColor(key, pt.r, pt.g, pt.b);
if(iter->first > 0)
{
uInsert(occupiedCells_, std::make_pair(n, OcTreeNodeInfo(iter->first, key, true)));
}
else
{
occupiedCells_.insert(std::make_pair(n, OcTreeNodeInfo(iter->first, key, true)));
}
}
}
}
UDEBUG("%d: occupied cells=%d free cells=%d", iter->first, (int)occupied_cells.size(), (int)free_cells.size());
// mark free cells only if not seen occupied in this cloud
for(octomap::KeySet::iterator it = free_cells.begin(), end=free_cells.end(); it!= end; ++it)
{
if (occupied_cells.find(*it) == occupied_cells.end() &&
ground_cells.find(*it) == ground_cells.end())
{
octomap::ColorOcTreeNode * n = octree_->updateNode(*it, false);
if(n)
{
std::map<octomap::ColorOcTreeNode*, OcTreeNodeInfo>::iterator gter;
gter = occupiedCells_.find(n);
if(gter != occupiedCells_.end() && gter->second.isObstacle_)
{
occupiedCells_.erase(gter);
}
}
}
}
// compress map
//octree_->prune();
// ignore negative ids as they are temporary clouds
if(iter->first > 0)
{
addedNodes_.insert(*iter);
}
UDEBUG("%d: end", iter->first);
}
else
{
UDEBUG("Did not find %d in cache", iter->first);
}
}
cache_.clear();
cacheClouds_.clear();
cacheViewPoints_.clear();
if(!fullUpdate_)
{
cache_.clear();
cacheClouds_.clear();
cacheViewPoints_.clear();
}
}
void HSVtoRGB( float *r, float *g, float *b, float h, float s, float v )
@@ -494,9 +510,15 @@ pcl::PointCloud<pcl::PointXYZRGB>::Ptr OctoMap::createCloud(
return cloud;
}
cv::Mat OctoMap::createProjectionMap(float & xMin, float & yMin, float & gridCellSize, float minGridSize)
cv::Mat OctoMap::createProjectionMap(float & xMin, float & yMin, float & gridCellSize, float minGridSize, unsigned int treeDepth)
{
gridCellSize = octree_->getResolution();
UASSERT(treeDepth <= octree_->getTreeDepth());
if(treeDepth == 0)
{
treeDepth = octree_->getTreeDepth();
}
gridCellSize = octree_->getNodeSize(treeDepth);
pcl::PointCloud<pcl::PointXYZ>::Ptr ground(new pcl::PointCloud<pcl::PointXYZ>);
pcl::PointCloud<pcl::PointXYZ>::Ptr obstacles(new pcl::PointCloud<pcl::PointXYZ>);
@@ -505,16 +527,15 @@ cv::Mat OctoMap::createProjectionMap(float & xMin, float & yMin, float & gridCel
obstacles->resize(octree_->size());
int gi=0;
int oi=0;
for (octomap::ColorOcTree::iterator it = octree_->begin(octree_->getTreeDepth()); it != octree_->end(); ++it)
for (octomap::ColorOcTree::iterator it = octree_->begin(treeDepth); it != octree_->end(); ++it)
{
octomap::point3d pt = octree_->keyToCoord(it.getKey());
if(octree_->isNodeOccupied(*it))
{
octomap::point3d pt = octree_->keyToCoord(it.getKey());
(*obstacles)[oi++] = pcl::PointXYZ(pt.x()-gridCellSize/2.0f, pt.y()-gridCellSize/2.0f, 0); // projected on ground
}
else
{
octomap::point3d pt = octree_->keyToCoord(it.getKey());
(*ground)[gi++] = pcl::PointXYZ(pt.x()-gridCellSize/2.0f, pt.y()-gridCellSize/2.0f, 0); // projected on ground
}
}
@@ -531,17 +552,19 @@ cv::Mat OctoMap::createProjectionMap(float & xMin, float & yMin, float & gridCel
}
cv::Mat obstaclesMat = cv::Mat(1, (int)obstacles->size(), CV_32FC2);
cv::Vec2f * ptr = obstaclesMat.ptr<cv::Vec2f>(0,0);
for(unsigned int i=0;i<obstacles->size(); ++i)
{
obstaclesMat.at<cv::Vec2f>(i)[0] = obstacles->at(i).x;
obstaclesMat.at<cv::Vec2f>(i)[1] = obstacles->at(i).y;
ptr[i][0] = obstacles->at(i).x;
ptr[i][1] = obstacles->at(i).y;
}
cv::Mat groundMat = cv::Mat(1, (int)ground->size(), CV_32FC2);
ptr = groundMat.ptr<cv::Vec2f>(0,0);
for(unsigned int i=0;i<ground->size(); ++i)
{
groundMat.at<cv::Vec2f>(i)[0] = ground->at(i).x;
groundMat.at<cv::Vec2f>(i)[1] = ground->at(i).y;
ptr[i][0] = ground->at(i).x;
ptr[i][1] = ground->at(i).y;
}
std::map<int, Transform> poses;

29
corelib/src/util3d_mapping.cpp
View File

@@ -877,6 +877,35 @@ cv::Mat convertMap2Image8U(const cv::Mat & map8S)
return map8U;
}
cv::Mat erodeMap(const cv::Mat & map)
{
UASSERT(map.type() == CV_8SC1);
cv::Mat erodedMap = map.clone();
for(int i=0; i<map.rows; ++i)
{
for(int j=0; j<map.cols; ++j)
{
if(map.at<char>(i, j) == 100)
{
// remove obstacles which touch at least 3 empty cells but not unknown cells
int touchEmpty = (map.at<char>(i+1, j) == 0?1:0) +
(map.at<char>(i-1, j) == 0?1:0) +
(map.at<char>(i, j+1) == 0?1:0) +
(map.at<char>(i, j-1) == 0?1:0);
if(touchEmpty>=3 && map.at<char>(i+1, j) != -1 &&
map.at<char>(i-1, j) != -1 &&
map.at<char>(i, j+1) != -1 &&
map.at<char>(i, j-1) != -1)
{
erodedMap.at<char>(i, j) = 0; // empty
}
}
}
}
return erodedMap;
}
}
}