i/rtabmap
1
0
Fork 0
mirror of https://github.com/introlab/rtabmap.git synced 2026-10-06 01:57:45 +08:00
Code Issues Packages Projects Releases Wiki Activity
Files
efa5afeba77f441ecfc02e78c1ed793c9141be45
rtabmap/corelib/src/global_map/OctoMap.cpp
T

1067 lines
32 KiB
C++
Raw Blame History

/*
Copyright (c) 2010-2016, Mathieu Labbe - IntRoLab - Universite de Sherbrooke
All rights reserved.
Redistribution and use in source and binary forms, with or without
modification, are permitted provided that the following conditions are met:
* Redistributions of source code must retain the above copyright
notice, this list of conditions and the following disclaimer.
* Redistributions in binary form must reproduce the above copyright
notice, this list of conditions and the following disclaimer in the
documentation and/or other materials provided with the distribution.
* Neither the name of the Universite de Sherbrooke nor the
names of its contributors may be used to endorse or promote products
derived from this software without specific prior written permission.
THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY
DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
(INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
(INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
*/
#include <rtabmap/core/global_map/OctoMap.h>
#include <rtabmap/utilite/ULogger.h>
#include <rtabmap/utilite/UStl.h>
#include <rtabmap/utilite/UTimer.h>
#include <rtabmap/core/util3d_transforms.h>
#include <rtabmap/core/util3d_filtering.h>
#include <rtabmap/core/util3d_mapping.h>
#include <rtabmap/core/util2d.h>
#include <pcl/common/transforms.h>
namespace rtabmap {
//////////////////////////////////////
// RtabmapColorOcTree
//////////////////////////////////////
RtabmapColorOcTreeNode* RtabmapColorOcTreeNode::getChild(unsigned int i) {
#ifdef OCTOMAP_PRE_18
return static_cast<RtabmapColorOcTreeNode*> (OcTreeNode::getChild(i));
#else
UFATAL("This function should not be used with octomap >= 1.8");
return 0;
#endif
}
const RtabmapColorOcTreeNode* RtabmapColorOcTreeNode::getChild(unsigned int i) const {
#ifdef OCTOMAP_PRE_18
return static_cast<const RtabmapColorOcTreeNode*> (OcTreeNode::getChild(i));
#else
UFATAL("This function should not be used with octomap >= 1.8");
return 0;
#endif
}
//octomap <1.8
bool RtabmapColorOcTreeNode::pruneNode() {
#ifdef OCTOMAP_PRE_18
// checks for equal occupancy only, color ignored
if (!this->collapsible()) return false;
// set occupancy value
setLogOdds(getChild(0)->getLogOdds());
// set color to average color
if (isColorSet()) color = getAverageChildColor();
// delete children
for (unsigned int i=0;i<8;i++) {
delete children[i];
}
delete[] children;
children = NULL;
return true;
#else
UFATAL("This function should not be used with octomap >= 1.8");
return false;
#endif
}
//octomap <1.8
void RtabmapColorOcTreeNode::expandNode() {
#ifdef OCTOMAP_PRE_18
assert(!hasChildren());
for (unsigned int k=0; k<8; k++) {
createChild(k);
children[k]->setValue(value);
getChild(k)->setColor(color);
}
#else
UFATAL("This function should not be used with octomap >= 1.8");
#endif
}
//octomap <1.8
bool RtabmapColorOcTreeNode::createChild(unsigned int i) {
#ifdef OCTOMAP_PRE_18
if (children == NULL) allocChildren();
children[i] = new RtabmapColorOcTreeNode();
return true;
#else
UFATAL("This function should not be used with octomap >= 1.8");
return false;
#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();
};
RtabmapColorOcTreeNode* RtabmapColorOcTree::setNodeColor(const octomap::OcTreeKey& key,
uint8_t r,
uint8_t g,
uint8_t b) {
RtabmapColorOcTreeNode* n = search (key);
if (n != 0) {
n->setColor(r, g, b);
}
return n;
}
bool RtabmapColorOcTree::pruneNode(RtabmapColorOcTreeNode* node) {
#ifndef OCTOMAP_PRE_18
if (!isNodeCollapsible(node))
return false;
// set value to children's values (all assumed equal)
node->copyData(*(getNodeChild(node, 0)));
if (node->isColorSet()) // TODO check
node->setColor(node->getAverageChildColor());
// delete children
for (unsigned int i=0;i<8;i++) {
deleteNodeChild(node, i);
}
delete[] node->children;
node->children = NULL;
return true;
#else
UFATAL("This function should not be used with octomap < 1.8");
return false;
#endif
}
bool RtabmapColorOcTree::isNodeCollapsible(const RtabmapColorOcTreeNode* node) const{
#ifndef OCTOMAP_PRE_18
// all children must exist, must not have children of
// their own and have the same occupancy probability
if (!nodeChildExists(node, 0))
return false;
const RtabmapColorOcTreeNode* firstChild = getNodeChild(node, 0);
if (nodeHasChildren(firstChild))
return false;
for (unsigned int i = 1; i<8; i++) {
// compare nodes only using their occupancy, ignoring color for pruning
if (!nodeChildExists(node, i) || nodeHasChildren(getNodeChild(node, i)) || !(getNodeChild(node, i)->getValue() == firstChild->getValue()))
return false;
}
return true;
#else
UFATAL("This function should not be used with octomap < 1.8");
return false;
#endif
}
RtabmapColorOcTreeNode* RtabmapColorOcTree::averageNodeColor(const octomap::OcTreeKey& key,
uint8_t r,
uint8_t g,
uint8_t b) {
RtabmapColorOcTreeNode* n = search(key);
if (n != 0) {
if (n->isColorSet()) {
RtabmapColorOcTreeNode::Color prev_color = n->getColor();
n->setColor((prev_color.r + r)/2, (prev_color.g + g)/2, (prev_color.b + b)/2);
}
else {
n->setColor(r, g, b);
}
}
return n;
}
RtabmapColorOcTreeNode* RtabmapColorOcTree::integrateNodeColor(const octomap::OcTreeKey& key,
uint8_t r,
uint8_t g,
uint8_t b) {
RtabmapColorOcTreeNode* n = search (key);
if (n != 0) {
if (n->isColorSet()) {
RtabmapColorOcTreeNode::Color prev_color = n->getColor();
double node_prob = n->getOccupancy();
uint8_t new_r = (uint8_t) ((double) prev_color.r * node_prob
+ (double) r * (0.99-node_prob));
uint8_t new_g = (uint8_t) ((double) prev_color.g * node_prob
+ (double) g * (0.99-node_prob));
uint8_t new_b = (uint8_t) ((double) prev_color.b * node_prob
+ (double) b * (0.99-node_prob));
n->setColor(new_r, new_g, new_b);
}
else {
n->setColor(r, g, b);
}
}
return n;
}
void RtabmapColorOcTree::updateInnerOccupancy() {
this->updateInnerOccupancyRecurs(this->root, 0);
}
void RtabmapColorOcTree::updateInnerOccupancyRecurs(RtabmapColorOcTreeNode* node, unsigned int depth) {
#ifndef OCTOMAP_PRE_18
// only recurse and update for inner nodes:
if (nodeHasChildren(node)){
// return early for last level:
if (depth < this->tree_depth){
for (unsigned int i=0; i<8; i++) {
if (nodeChildExists(node, i)) {
updateInnerOccupancyRecurs(getNodeChild(node, i), depth+1);
}
}
}
node->updateOccupancyChildren();
node->updateColorChildren();
node->updateOccupancyTypeChildren();
}
#else
// only recurse and update for inner nodes:
if (node->hasChildren()){
// return early for last level:
if (depth < this->tree_depth){
for (unsigned int i=0; i<8; i++) {
if (node->childExists(i)) {
updateInnerOccupancyRecurs(node->getChild(i), depth+1);
}
}
}
node->updateOccupancyChildren();
node->updateColorChildren();
node->updateOccupancyTypeChildren();
}
#endif
}
RtabmapColorOcTree::StaticMemberInitializer::StaticMemberInitializer() {
RtabmapColorOcTree* tree = new RtabmapColorOcTree(0.1);
#ifndef OCTOMAP_PRE_18
tree->clearKeyRays();
#endif
AbstractOcTree::registerTreeType(tree);
}
#ifndef _WIN32
// On Windows, the app freezes on start if the following is defined
RtabmapColorOcTree::StaticMemberInitializer RtabmapColorOcTree::RtabmapColorOcTreeMemberInit;
#endif
//////////////////////////////////////
// OctoMap
//////////////////////////////////////
OctoMap::OctoMap(const LocalGridCache * cache, const ParametersMap & parameters) :
GlobalMap(cache, parameters),
hasColor_(false),
rangeMax_(Parameters::defaultGridRangeMax()),
rayTracing_(Parameters::defaultGridRayTracing()),
emptyFloodFillDepth_(Parameters::defaultGridGlobalFloodFillDepth())
{
octree_ = new RtabmapColorOcTree(cellSize_);
if(occupancyThr_ <= 0.0f)
{
UWARN("Cannot set %s to null for OctoMap, using default value %f instead.",
Parameters::kGridGlobalOccupancyThr().c_str(),
Parameters::defaultGridGlobalOccupancyThr());
occupancyThr_ = Parameters::defaultGridGlobalOccupancyThr();
}
UDEBUG("occupancyThr_=%f", occupancyThr_);
UDEBUG("probHit_=%f", probability(logOddsHit_));
UDEBUG("probMiss_=%f", probability(logOddsMiss_));
UDEBUG("probClampingMin_=%f", probability(logOddsClampingMin_));
UDEBUG("probClampingMax_=%f", probability(logOddsClampingMax_));
octree_->setOccupancyThres(occupancyThr_);
octree_->setProbHit(probability(logOddsHit_));
octree_->setProbMiss(probability(logOddsMiss_));
octree_->setClampingThresMin(probability(logOddsClampingMin_));
octree_->setClampingThresMax(probability(logOddsClampingMax_));
Parameters::parse(parameters, Parameters::kGridRangeMax(), rangeMax_);
Parameters::parse(parameters, Parameters::kGridRayTracing(), rayTracing_);
Parameters::parse(parameters, Parameters::kGridGlobalFloodFillDepth(), emptyFloodFillDepth_);
UASSERT(emptyFloodFillDepth_<=16);
UDEBUG("rangeMax_ =%f", rangeMax_);
UDEBUG("rayTracing_ =%s", rayTracing_?"true":"false");
UDEBUG("emptyFloodFillDepth_=%d", emptyFloodFillDepth_);
}
OctoMap::~OctoMap()
{
this->clear();
delete octree_;
}
void OctoMap::clear()
{
octree_->clear();
hasColor_ = false;
GlobalMap::clear();
}
unsigned long OctoMap::getMemoryUsed() const
{
unsigned long memoryUsage = GlobalMap::getMemoryUsed();
// Note: size of OctoMap object is missing.
return memoryUsage;
}
bool OctoMap::isValidEmpty(RtabmapColorOcTree* octree_, unsigned int treeDepth,octomap::point3d startPosition)
{
auto nodePtr = octree_->search(startPosition.x(), startPosition.y(), startPosition.z(), treeDepth);
if(nodePtr != NULL)
{
if(!octree_->isNodeOccupied(*nodePtr))
{
return true;
}
}
return false;
}
octomap::point3d OctoMap::findCloseEmpty(RtabmapColorOcTree* octree_, unsigned int treeDepth,octomap::point3d startPosition)
{
//try current position
if(isValidEmpty(octree_,treeDepth,startPosition))
{
return startPosition;
}
//x pos
if(isValidEmpty(octree_,treeDepth,octomap::point3d(startPosition.x()+octree_->getNodeSize(treeDepth), startPosition.y(), startPosition.z())))
{
return octomap::point3d(startPosition.x()+octree_->getNodeSize(treeDepth), startPosition.y(), startPosition.z());
}
//x neg
if(isValidEmpty(octree_,treeDepth,octomap::point3d(startPosition.x()-octree_->getNodeSize(treeDepth), startPosition.y(), startPosition.z())))
{
return octomap::point3d(startPosition.x()-octree_->getNodeSize(treeDepth), startPosition.y(), startPosition.z());
}
//y pos
if(isValidEmpty(octree_,treeDepth,octomap::point3d(startPosition.x(), startPosition.y()+octree_->getNodeSize(treeDepth), startPosition.z())))
{
return octomap::point3d(startPosition.x(), startPosition.y()+octree_->getNodeSize(treeDepth), startPosition.z());
}
//y neg
if(isValidEmpty(octree_,treeDepth,octomap::point3d(startPosition.x()-octree_->getNodeSize(treeDepth), startPosition.y(), startPosition.z())))
{
return octomap::point3d(startPosition.x(), startPosition.y()-octree_->getNodeSize(treeDepth), startPosition.z());
}
//z pos
if(isValidEmpty(octree_,treeDepth,octomap::point3d(startPosition.x(), startPosition.y(), startPosition.z()+octree_->getNodeSize(treeDepth))))
{
return octomap::point3d(startPosition.x(), startPosition.y(), startPosition.z()+octree_->getNodeSize(treeDepth));
}
//z neg
if(isValidEmpty(octree_,treeDepth,octomap::point3d(startPosition.x(), startPosition.y(), startPosition.z()-octree_->getNodeSize(treeDepth))))
{
return octomap::point3d(startPosition.x(), startPosition.y(), startPosition.z()-octree_->getNodeSize(treeDepth));
}
//no valid position
return startPosition;
}
bool OctoMap::isNodeVisited(std::unordered_set<octomap::OcTreeKey,octomap::OcTreeKey::KeyHash> const & EmptyNodes,octomap::OcTreeKey const key)
{
for(auto it = EmptyNodes.find(key);it != EmptyNodes.end();it++)
{
if(*it == key)
{
return true;
}
}
return false;
}
void OctoMap::floodFill(RtabmapColorOcTree* octree_, unsigned int treeDepth,octomap::point3d startPosition, std::unordered_set<octomap::OcTreeKey, octomap::OcTreeKey::KeyHash> & EmptyNodes,std::queue<octomap::point3d>& positionToExplore)
{
auto key = octree_->coordToKey(startPosition,treeDepth);
if(!isNodeVisited(EmptyNodes,key))
{
if(isValidEmpty(octree_,treeDepth,startPosition))
{
EmptyNodes.insert(key);
positionToExplore.push(octomap::point3d(startPosition.x()+octree_->getNodeSize(treeDepth), startPosition.y(), startPosition.z()));
positionToExplore.push(octomap::point3d(startPosition.x()-octree_->getNodeSize(treeDepth), startPosition.y(), startPosition.z()));
positionToExplore.push(octomap::point3d(startPosition.x(), startPosition.y()+octree_->getNodeSize(treeDepth), startPosition.z()));
positionToExplore.push(octomap::point3d(startPosition.x(), startPosition.y()-octree_->getNodeSize(treeDepth), startPosition.z()));
positionToExplore.push(octomap::point3d(startPosition.x(), startPosition.y(), startPosition.z()+octree_->getNodeSize(treeDepth)));
positionToExplore.push(octomap::point3d(startPosition.x(), startPosition.y(), startPosition.z()-octree_->getNodeSize(treeDepth)));
}
}
}
std::unordered_set<octomap::OcTreeKey, octomap::OcTreeKey::KeyHash> OctoMap::findEmptyNode(RtabmapColorOcTree* octree_, unsigned int treeDepth, octomap::point3d startPosition)
{
std::unordered_set<octomap::OcTreeKey, octomap::OcTreeKey::KeyHash> exploreNode;
std::queue<octomap::point3d> positionToExplore;
startPosition = findCloseEmpty(octree_,treeDepth, startPosition);
floodFill(octree_, treeDepth, startPosition, exploreNode, positionToExplore);
while(!positionToExplore.empty())
{
floodFill(octree_, treeDepth, positionToExplore.front(), exploreNode, positionToExplore);
positionToExplore.pop();
}
return exploreNode;
}
void OctoMap::assemble(const std::list<std::pair<int, Transform> > & newPoses)
{
// Original version from A. Hornung:
// https://github.com/OctoMap/octomap_mapping/blob/jade-devel/octomap_server/src/OctomapServer.cpp#L356
//
int lastId = assembledNodes().size()?assembledNodes().rbegin()->first:0;
UDEBUG("Last id = %d", lastId);
UDEBUG("newPoses = %d", (int)newPoses.size());
if(!newPoses.empty())
{
float rangeMaxSqrd = rangeMax_*rangeMax_;
float cellSize = octree_->getResolution();
int not3DCount = 0;
int not3DFirstId = 0;
int not3DGroundType = 0;
int not3DObstaclesType = 0;
int not3DEmptyType = 0;
for(std::list<std::pair<int, Transform> >::const_iterator iter=newPoses.begin(); iter!=newPoses.end(); ++iter)
{
std::map<int, LocalGrid>::const_iterator localGridIter;
localGridIter = cache().find(iter->first);
if(localGridIter != cache().end())
{
cv::Mat ground = localGridIter->second.groundCells;
cv::Mat obstacles = localGridIter->second.obstacleCells;
cv::Mat emptyCells = localGridIter->second.emptyCells;
if(!localGridIter->second.is3D())
{
if(++not3DCount == 1)
{
not3DFirstId = iter->first;
not3DGroundType = ground.type();
not3DObstaclesType = obstacles.type();
not3DEmptyType = emptyCells.type();
}
continue;
}
UDEBUG("Adding %d to octomap (resolution=%f)", iter->first, octree_->getResolution());
octomap::point3d sensorOrigin(iter->second.x(), iter->second.y(), iter->second.z());
sensorOrigin += octomap::point3d(localGridIter->second.viewPoint.x, localGridIter->second.viewPoint.y, localGridIter->second.viewPoint.z);
updateMinMax(sensorOrigin);
octomap::OcTreeKey tmpKey;
if (!octree_->coordToKeyChecked(sensorOrigin, tmpKey))
{
UERROR("Could not generate Key for origin (%f,%f,%f)", sensorOrigin.x(), sensorOrigin.y(), sensorOrigin.z());
}
bool computeRays = rayTracing_ && emptyCells.empty();
// instead of direct scan insertion, compute update to filter ground:
octomap::KeySet free_cells;
// insert ground points only as free:
unsigned int maxGroundPts = ground.cols;
UDEBUG("%d: compute free cells (from %d ground points)", iter->first, (int)maxGroundPts);
Eigen::Affine3f t = iter->second.toEigen3f();
LaserScan tmpGround = LaserScan::backwardCompatibility(ground);
UASSERT(tmpGround.size() == (int)maxGroundPts);
for (unsigned int i=0; i<maxGroundPts; ++i)
{
pcl::PointXYZRGB pt;
pt = util3d::laserScanToPointRGB(tmpGround, i);
pt = pcl::transformPoint(pt, t);
octomap::point3d point(pt.x, pt.y, pt.z);
bool ignoreOccupiedCell = false;
if(rangeMaxSqrd > 0.0f)
{
octomap::point3d v(pt.x - cellSize - sensorOrigin.x(), pt.y - cellSize - sensorOrigin.y(), pt.z - cellSize - sensorOrigin.z());
if(v.norm_sq() > rangeMaxSqrd)
{
// compute new point to max range
v.normalize();
v*=rangeMax_;
point = sensorOrigin + v;
ignoreOccupiedCell=true;
}
}
if(!ignoreOccupiedCell)
{
// occupied endpoint
octomap::OcTreeKey key;
if (octree_->coordToKeyChecked(point, key))
{
if(iter->first >0 && iter->first<lastId)
{
RtabmapColorOcTreeNode * n = octree_->search(key);
if(n && n->getNodeRefId() > 0 && n->getNodeRefId() > iter->first)
{
// The cell has been updated from more recent node, don't update the cell
continue;
}
}
updateMinMax(point);
RtabmapColorOcTreeNode * n = octree_->updateNode(key, true);
if(n)
{
if(!hasColor_ && !(pt.r ==0 && pt.g == 0 && pt.b == 0) && !(pt.r ==255 && pt.g == 255 && pt.b == 255))
{
hasColor_ = true;
}
octree_->averageNodeColor(key, pt.r, pt.g, pt.b);
if(iter->first > 0)
{
n->setNodeRefId(iter->first);
n->setPointRef(point);
}
n->setOccupancyType(RtabmapColorOcTreeNode::kTypeGround);
}
}
}
// only clear space (ground points)
octomap::KeyRay keyRay;
if (computeRays &&
(iter->first < 0 || iter->first>lastId) &&
octree_->computeRayKeys(sensorOrigin, point, keyRay))
{
free_cells.insert(keyRay.begin(), keyRay.end());
}
}
UDEBUG("%d: ground cells=%d free cells=%d", iter->first, (int)maxGroundPts, (int)free_cells.size());
// all other points: free on ray, occupied on endpoint:
unsigned int maxObstaclePts = obstacles.cols;
UDEBUG("%d: compute occupied cells (from %d obstacle points)", iter->first, (int)maxObstaclePts);
LaserScan tmpObstacle = LaserScan::backwardCompatibility(obstacles);
UASSERT(tmpObstacle.size() == (int)maxObstaclePts);
for (unsigned int i=0; i<maxObstaclePts; ++i)
{
pcl::PointXYZRGB pt;
pt = util3d::laserScanToPointRGB(tmpObstacle, i);
pt = pcl::transformPoint(pt, t);
octomap::point3d point(pt.x, pt.y, pt.z);
bool ignoreOccupiedCell = false;
if(rangeMaxSqrd > 0.0f)
{
octomap::point3d v(pt.x - cellSize - sensorOrigin.x(), pt.y - cellSize - sensorOrigin.y(), pt.z - cellSize - sensorOrigin.z());
if(v.norm_sq() > rangeMaxSqrd)
{
// compute new point to max range
v.normalize();
v*=rangeMax_;
point = sensorOrigin + v;
ignoreOccupiedCell=true;
}
}
if(!ignoreOccupiedCell)
{
// occupied endpoint
octomap::OcTreeKey key;
if (octree_->coordToKeyChecked(point, key))
{
if(iter->first >0 && iter->first<lastId)
{
RtabmapColorOcTreeNode * n = octree_->search(key);
if(n && n->getNodeRefId() > 0 && n->getNodeRefId() > iter->first)
{
// The cell has been updated from more recent node, don't update the cell
continue;
}
}
updateMinMax(point);
RtabmapColorOcTreeNode * n = octree_->updateNode(key, true);
if(n)
{
if(!hasColor_ && !(pt.r ==0 && pt.g == 0 && pt.b == 0) && !(pt.r ==255 && pt.g == 255 && pt.b == 255))
{
hasColor_ = true;
}
octree_->averageNodeColor(key, pt.r, pt.g, pt.b);
if(iter->first > 0)
{
n->setNodeRefId(iter->first);
n->setPointRef(point);
}
n->setOccupancyType(RtabmapColorOcTreeNode::kTypeObstacle);
}
}
}
// free cells
octomap::KeyRay keyRay;
if (computeRays &&
(iter->first < 0 || iter->first>lastId) &&
octree_->computeRayKeys(sensorOrigin, point, keyRay))
{
free_cells.insert(keyRay.begin(), keyRay.end());
}
}
UDEBUG("%d: occupied cells=%d free cells=%d", iter->first, (int)maxObstaclePts, (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(iter->first > 0)
{
RtabmapColorOcTreeNode * n = octree_->search(*it);
if(n && n->getNodeRefId() > 0 && n->getNodeRefId() >= iter->first)
{
// The cell has been updated from current node or more recent node, don't update the cell
continue;
}
}
RtabmapColorOcTreeNode * n = octree_->updateNode(*it, false, true);
if(n && n->getOccupancyType() == RtabmapColorOcTreeNode::kTypeUnknown)
{
n->setOccupancyType(RtabmapColorOcTreeNode::kTypeEmpty);
if(iter->first > 0)
{
n->setNodeRefId(iter->first);
}
}
}
// all empty cells
if(emptyCells.cols)
{
unsigned int maxEmptyPts = emptyCells.cols;
UDEBUG("%d: compute free cells (from %d empty points)", iter->first, (int)maxEmptyPts);
LaserScan tmpEmpty = LaserScan::backwardCompatibility(emptyCells);
UASSERT(tmpEmpty.size() == (int)maxEmptyPts);
for (unsigned int i=0; i<maxEmptyPts; ++i)
{
pcl::PointXYZ pt;
pt = util3d::laserScanToPoint(tmpEmpty, i);
pt = pcl::transformPoint(pt, t);
octomap::point3d point(pt.x, pt.y, pt.z);
bool ignoreCell = false;
if(rangeMaxSqrd > 0.0f)
{
octomap::point3d v(pt.x - sensorOrigin.x(), pt.y - sensorOrigin.y(), pt.z - sensorOrigin.z());
if(v.norm_sq() > rangeMaxSqrd)
{
ignoreCell=true;
}
}
if(!ignoreCell)
{
octomap::OcTreeKey key;
if (octree_->coordToKeyChecked(point, key))
{
if(iter->first >0)
{
RtabmapColorOcTreeNode * n = octree_->search(key);
if(n && n->getNodeRefId() > 0 && n->getNodeRefId() >= iter->first)
{
// The cell has been updated from current node or more recent node, don't update the cell
continue;
}
}
updateMinMax(point);
RtabmapColorOcTreeNode * n = octree_->updateNode(key, false, true);
if(n && n->getOccupancyType() == RtabmapColorOcTreeNode::kTypeUnknown)
{
n->setOccupancyType(RtabmapColorOcTreeNode::kTypeEmpty);
if(iter->first > 0)
{
n->setNodeRefId(iter->first);
}
}
}
}
}
}
if(emptyCells.cols || !free_cells.empty())
{
octree_->updateInnerOccupancy();
}
// compress map
//if(newPoses.size() > 1)
//{
// octree_->prune();
//}
addAssembledNode(iter->first, iter->second);
UDEBUG("%d: end", iter->first);
}
else
{
UDEBUG("Did not find %d in cache", iter->first);
}
}
if(not3DCount)
{
UWARN("It seems the local occupancy grids are not 3d, cannot update OctoMap! "
"(%d local grid(s) ignored, first one (id=%d) had ground type=%d, obstacles type=%d, empty type=%d)",
not3DCount, not3DFirstId, not3DGroundType, not3DObstaclesType, not3DEmptyType);
}
}
if(emptyFloodFillDepth_>0)
{
UTimer t;
auto key = octree_->coordToKey(0, 0, 0, emptyFloodFillDepth_);
auto pos = octree_->keyToCoord(key);
std::unordered_set<octomap::OcTreeKey, octomap::OcTreeKey::KeyHash> EmptyNodes = findEmptyNode(octree_,emptyFloodFillDepth_, pos);
std::vector<octomap::OcTreeKey> nodeToDelete;
for (RtabmapColorOcTree::iterator it = octree_->begin_leafs(emptyFloodFillDepth_); it != octree_->end_leafs(); ++it)
{
if(!octree_->isNodeOccupied(*it))
{
if(!isNodeVisited(EmptyNodes,it.getKey()))
{
nodeToDelete.push_back(it.getKey());
}
}
}
for(unsigned int y=0; y < nodeToDelete.size(); y++)
{
octree_->deleteNode(nodeToDelete[y],emptyFloodFillDepth_);
}
UDEBUG("Flood Fill: deleted %d empty cells (%fs)", (int)nodeToDelete.size(), t.ticks());
}
}
void OctoMap::updateMinMax(const octomap::point3d & point)
{
if(point.x() < minValues_[0])
{
minValues_[0] = point.x();
}
if(point.y() < minValues_[1])
{
minValues_[1] = point.y();
}
if(point.z() < minValues_[2])
{
minValues_[2] = point.z();
}
if(point.x() > maxValues_[0])
{
maxValues_[0] = point.x();
}
if(point.y() > maxValues_[1])
{
maxValues_[1] = point.y();
}
if(point.z() > maxValues_[2])
{
maxValues_[2] = point.z();
}
}
pcl::PointCloud<pcl::PointXYZRGB>::Ptr OctoMap::createCloud(
unsigned int treeDepth,
std::vector<int> * obstacleIndices,
std::vector<int> * emptyIndices,
std::vector<int> * groundIndices,
bool originalRefPoints,
std::vector<int> * frontierIndices,
std::vector<double> * cloudProb) const
{
UASSERT(treeDepth <= octree_->getTreeDepth());
pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud(new pcl::PointCloud<pcl::PointXYZRGB>);
if(cloudProb)
{
cloudProb->resize(octree_->size());
}
UDEBUG("depth=%d (maxDepth=%d) octree = %d",
(int)treeDepth, (int)octree_->getTreeDepth(), (int)octree_->size());
cloud->resize(octree_->size());
if(obstacleIndices)
{
obstacleIndices->resize(octree_->size());
}
if(emptyIndices)
{
emptyIndices->resize(octree_->size());
}
if(frontierIndices)
{
frontierIndices->resize(octree_->size());
}
if(groundIndices)
{
groundIndices->resize(octree_->size());
}
if(treeDepth == 0)
{
treeDepth = octree_->getTreeDepth();
}
double minZ = minValues_[2];
double maxZ = maxValues_[2];
bool addAllPoints = obstacleIndices == 0 && groundIndices == 0 && emptyIndices == 0;
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)
{
if(octree_->isNodeOccupied(*it) && (obstacleIndices != 0 || groundIndices != 0 || addAllPoints))
{
octomap::point3d pt = octree_->keyToCoord(it.getKey());
if(cloudProb)
{
(*cloudProb)[oi] = it->getOccupancy();
}
if(octree_->getTreeDepth() == it.getDepth() && hasColor_)
{
(*cloud)[oi] = pcl::PointXYZRGB(it->getColor().r, it->getColor().g, it->getColor().b);
}
else
{
// Gradiant color on z axis
float H = (maxZ - pt.z())*299.0f/(maxZ-minZ);
float r,g,b;
util2d::HSVtoRGB(&r, &g, &b, H, 1, 1);
(*cloud)[oi].r = r*255.0f;
(*cloud)[oi].g = g*255.0f;
(*cloud)[oi].b = b*255.0f;
}
if(originalRefPoints && it->getOccupancyType() > 0)
{
const octomap::point3d & p = it->getPointRef();
(*cloud)[oi].x = p.x();
(*cloud)[oi].y = p.y();
(*cloud)[oi].z = p.z();
}
else
{
(*cloud)[oi].x = pt.x()-halfCellSize;
(*cloud)[oi].y = pt.y()-halfCellSize;
(*cloud)[oi].z = pt.z();
}
if(it->getOccupancyType() == RtabmapColorOcTreeNode::kTypeGround)
{
if(groundIndices)
{
groundIndices->at(gi++) = oi;
}
}
else if(obstacleIndices)
{
obstacleIndices->at(si++) = oi;
}
++oi;
}
else if(!octree_->isNodeOccupied(*it) && (emptyIndices != 0 || addAllPoints || frontierIndices !=0))
{
octomap::point3d pt = octree_->keyToCoord(it.getKey());
if(cloudProb)
{
(*cloudProb)[oi] = it->getOccupancy();
}
if(frontierIndices !=0 &&
(!octree_->search( pt.x()+octree_->getNodeSize(treeDepth), pt.y(), pt.z(), treeDepth) || !octree_->search( pt.x()-octree_->getNodeSize(treeDepth), pt.y(), pt.z(), treeDepth) ||
!octree_->search( pt.x(), pt.y()+octree_->getNodeSize(treeDepth), pt.z(), treeDepth) || !octree_->search( pt.x(), pt.y()-octree_->getNodeSize(treeDepth), pt.z(), treeDepth) ||
!octree_->search( pt.x(), pt.y(), pt.z()+octree_->getNodeSize(treeDepth), treeDepth) || !octree_->search( pt.x(), pt.y(), pt.z()-octree_->getNodeSize(treeDepth), 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;
(*cloud)[oi].z = pt.z();
if(emptyIndices)
{
emptyIndices->at(ei++) = oi;
}
++oi;
}
}
cloud->resize(oi);
if(cloudProb)
{
cloudProb->resize(oi);
}
if(obstacleIndices)
{
obstacleIndices->resize(si);
UDEBUG("obstacle=%d", si);
}
if(emptyIndices)
{
emptyIndices->resize(ei);
UDEBUG("empty=%d", ei);
}
if(frontierIndices)
{
frontierIndices->resize(fi);
UDEBUG("frontier=%d", fi);
}
if(groundIndices)
{
groundIndices->resize(gi);
UDEBUG("ground=%d", gi);
}
UDEBUG("");
return cloud;
}
cv::Mat OctoMap::createProjectionMap(float & xMin, float & yMin, float & gridCellSize, float minGridSize, unsigned int treeDepth)
{
UDEBUG("minGridSize=%f, treeDepth=%d", minGridSize, (int)treeDepth);
UASSERT(treeDepth <= octree_->getTreeDepth());
if(treeDepth == 0)
{
treeDepth = octree_->getTreeDepth();
}
gridCellSize = octree_->getNodeSize(treeDepth);
cv::Mat obstaclesMat = cv::Mat(1, (int)octree_->size(), CV_32FC2);
cv::Mat groundMat = cv::Mat(1, (int)octree_->size(), CV_32FC2);
int gi=0;
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)
{
// projected on ground
oPtr[oi][0] = pt.x()-halfCellSize;
oPtr[oi][1] = pt.y()-halfCellSize;
++oi;
}
else
{
// projected on ground
gPtr[gi][0] = pt.x()-halfCellSize;
gPtr[gi][1] = pt.y()-halfCellSize;
++gi;
}
}
obstaclesMat = obstaclesMat(cv::Range::all(), cv::Range(0, oi));
groundMat = groundMat(cv::Range::all(), cv::Range(0, gi));
std::map<int, Transform> poses;
poses.insert(std::make_pair(1, Transform::getIdentity()));
std::map<int, std::pair<cv::Mat, cv::Mat> > maps;
maps.insert(std::make_pair(1, std::make_pair(groundMat, obstaclesMat)));
cv::Mat map = util3d::create2DMapFromOccupancyLocalMaps(
poses,
maps,
gridCellSize,
xMin, yMin,
minGridSize,
false);
UDEBUG("");
return map;
}
bool OctoMap::writeBinary(const std::string & path)
{
return octree_->writeBinary(path);
}
} /* namespace rtabmap */
Reference in New Issue View Git Blame Copy Permalink