diff --git a/corelib/include/rtabmap/core/OccupancyGrid.h b/corelib/include/rtabmap/core/OccupancyGrid.h index d56aa1e8..dc74e3d9 100644 --- a/corelib/include/rtabmap/core/OccupancyGrid.h +++ b/corelib/include/rtabmap/core/OccupancyGrid.h @@ -45,6 +45,8 @@ public: void setCellSize(float cellSize); float getCellSize() const {return cellSize_;} bool isGridFromDepth() const {return occupancyFromCloud_;} + bool isFullUpdate() const {return fullUpdate_;} + const std::map & addedNodes() const {return addedNodes_;} template typename pcl::PointCloud::Ptr segmentCloud( @@ -104,6 +106,7 @@ private: bool scan2dUnknownSpaceFilled_; double scan2dMaxUnknownSpaceFilledRange_; bool projRayTracing_; + bool fullUpdate_; std::map > cache_; cv::Mat map_; diff --git a/corelib/include/rtabmap/core/OctoMap.h b/corelib/include/rtabmap/core/OctoMap.h index aa33f3bc..a250c639 100644 --- a/corelib/include/rtabmap/core/OctoMap.h +++ b/corelib/include/rtabmap/core/OctoMap.h @@ -57,7 +57,7 @@ public: class RTABMAP_EXP OctoMap { public: - OctoMap(float voxelSize = 0.1f, float occupancyThr = 0.5f); + OctoMap(float voxelSize = 0.1f, float occupancyThr = 0.5f, bool fullUpdate = false); const std::map & addedNodes() const {return addedNodes_;} void addToCache(int nodeId, @@ -81,7 +81,8 @@ public: float & xMin, float & yMin, float & gridCellSize, - float minGridSize); + float minGridSize = 0.0f, + unsigned int treeDepth = 0); bool writeBinary(const std::string & path); @@ -97,6 +98,7 @@ private: std::map addedNodes_; octomap::KeyRay keyRay_; bool hasColor_; + bool fullUpdate_; }; } /* namespace rtabmap */ diff --git a/corelib/include/rtabmap/core/Parameters.h b/corelib/include/rtabmap/core/Parameters.h index d11ce79f..12293652 100644 --- a/corelib/include/rtabmap/core/Parameters.h +++ b/corelib/include/rtabmap/core/Parameters.h @@ -505,6 +505,7 @@ class RTABMAP_EXP Parameters RTABMAP_PARAM(Grid, Scan2dUnknownSpaceFilled, bool, false, "Unknown space filled. Only used with 2D laser scans."); RTABMAP_PARAM(Grid, Scan2dMaxFilledRange, float, 4.0, "Unknown space filled maximum range. If 0, the laser scan maximum range is used."); RTABMAP_PARAM(Grid, ProjRayTracing, bool, true, uFormat("[%s=false] 2D ray tracing is done for each projected obstacle, filling unknown space between the sensor and obstacles.", kGrid3D().c_str())); + RTABMAP_PARAM(Grid, FullUpdate, bool, true, "When the graph is changed, the whole map will be reconstructed instead of moving individually each cells of the map. Also, data added to cache won't be released after updating the map. This process is longer but more robust to drift that would erase some parts of the map when it should not."); public: virtual ~Parameters(); diff --git a/corelib/include/rtabmap/core/util3d_mapping.h b/corelib/include/rtabmap/core/util3d_mapping.h index 6c137eb4..01348992 100644 --- a/corelib/include/rtabmap/core/util3d_mapping.h +++ b/corelib/include/rtabmap/core/util3d_mapping.h @@ -116,6 +116,8 @@ void RTABMAP_EXP rayTrace(const cv::Point2i & start, cv::Mat RTABMAP_EXP convertMap2Image8U(const cv::Mat & map8S); +cv::Mat RTABMAP_EXP erodeMap(const cv::Mat & map); + template typename pcl::PointCloud::Ptr projectCloudOnXYPlane( const typename pcl::PointCloud & cloud); diff --git a/corelib/src/OccupancyGrid.cpp b/corelib/src/OccupancyGrid.cpp index 4b92af93..2ce7dbdd 100644 --- a/corelib/src/OccupancyGrid.cpp +++ b/corelib/src/OccupancyGrid.cpp @@ -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 & posesIn, float minMapSize, float footprintRadius) @@ -391,7 +393,7 @@ void OccupancyGrid::update(const std::map & 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 & posesIn, float minMa std::map emptyLocalMaps; std::map 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 transforms; for(std::map::iterator iter=addedNodes_.begin(); iter!=addedNodes_.end(); ++iter) @@ -448,75 +450,78 @@ void OccupancyGrid::update(const std::map & 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 > tmpIndices; - for(std::map >::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 > tmpIndices; + for(std::map >::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(y,x); - int nodeId = (int)info[0]; - if(nodeId > 0 && map_.at(y,x) >= 0) + if(iter->second.first) { - std::map::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(y,x); + int nodeId = (int)info[0]; + if(nodeId > 0 && map_.at(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 >::iterator jter = tmpIndices.find(nodeId); - if(map_.at(y, x) == 0) + std::map::iterator tter = transforms.find(nodeId); + if(tter != transforms.end() && !uContains(cache_, nodeId)) { - // ground - std::map::iterator iter = emptyLocalMaps.find(nodeId); - UASSERT(iter != emptyLocalMaps.end()); - UASSERT(jter->second.first < iter->second.cols); - float * ptf = iter->second.ptr(0,jter->second.first++); - ptf[0] = pt.x; - ptf[1] = pt.y; - } - else - { - // obstacle - std::map::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(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 >::iterator jter = tmpIndices.find(nodeId); + if(map_.at(y, x) == 0) + { + // ground + std::map::iterator iter = emptyLocalMaps.find(nodeId); + UASSERT(iter != emptyLocalMaps.end()); + UASSERT(jter->second.first < iter->second.cols); + float * ptf = iter->second.ptr(0,jter->second.first++); + ptf[0] = pt.x; + ptf[1] = pt.y; + } + else + { + // obstacle + std::map::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(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 & 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 > poses; - // place negative poses at the end - for(std::map::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::const_iterator iter=posesIn.upper_bound(lastId); iter!=posesIn.end(); ++iter) { poses.push_back(*iter); } + // insert negative after + for(std::map::const_iterator iter=posesIn.begin(); iter!=posesIn.end(); ++iter) + { + if(iter->first < 0) + { + poses.push_back(*iter); + } + else + { + break; + } + } } for(std::list >::const_iterator iter = poses.begin(); iter!=poses.end(); ++iter) { @@ -662,9 +678,12 @@ void OccupancyGrid::update(const std::map & 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 & 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 & 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 & 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 & posesIn, float minMa for(int i=0; isecond.cols; ++i) { float * ptf = iter->second.ptr(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(pt.y, pt.x); - if(value != -2) + if(value != -2 && (!incrementalGraphUpdate || value==-1)) { float * info = mapInfo.ptr(pt.y, pt.x); int nodeId = (int)info[0]; @@ -792,8 +818,8 @@ void OccupancyGrid::update(const std::map & 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 & 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 & posesIn, float minMa for(int i=0; isecond.cols; ++i) { float * ptf = jter->second.ptr(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 & posesIn, float minMa } } - // fill holes and put footprint values to empty (0) - //pcl::PointCloud::Ptr cloud(new pcl::PointCloud); - //cloud->resize(map.rows*map.cols); - //int oi=0; - for(int i=1; i= cellSize_*1.5f || incrementalGraphUpdate) { - for(int j=1; j(i, j); - if(value == -2) + for(int j=1; j(i, j); + if(value == -2) + { + value = 0; + } - char sum = (map.at(i+1, j) != -1?1:0) + - (map.at(i-1, j) != -1?1:0) + - (map.at(i, j+1) != -1?1:0) + - (map.at(i, j-1) != -1?1:0); - if(value == -1 && sum >=3) - { - value = 0; - } + if(incrementalGraphUpdate && value == -1) + { + float * info = mapInfo.ptr(i, j); - //float * info = mapInfo.ptr(i,j); - //if(info[0] > 0) - //{ - // cloud->at(oi).x = info[1]; - // cloud->at(oi).y = info[2]; - // oi++; - //} + // fill obstacle + if(map.at(i+1, j) == 100 && map.at(i-1, j) == 100) + { + value = 100; + // associate with the nearest pose + if(mapInfo.ptr(i+1, j)[0]>0.0f) + { + info[0] = mapInfo.ptr(i+1, j)[0]; + info[1] = float(j) * cellSize_ + xMin; + info[2] = float(i) * cellSize_ + yMin; + std::map >::iterator cter = cellCount_.find(int(info[0])); + UASSERT(cter!=cellCount_.end()); + cter->second.second+=1; + } + else if(mapInfo.ptr(i-1, j)[0]>0.0f) + { + info[0] = mapInfo.ptr(i-1, j)[0]; + info[1] = float(j) * cellSize_ + xMin; + info[2] = float(i) * cellSize_ + yMin; + std::map >::iterator cter = cellCount_.find(int(info[0])); + UASSERT(cter!=cellCount_.end()); + cter->second.second+=1; + } + } + else if(map.at(i, j+1) == 100 && map.at(i, j-1) == 100) + { + value = 100; + // associate with the nearest pose + if(mapInfo.ptr(i, j+1)[0]>0.0f) + { + info[0] = mapInfo.ptr(i, j+1)[0]; + info[1] = float(j) * cellSize_ + xMin; + info[2] = float(i) * cellSize_ + yMin; + std::map >::iterator cter = cellCount_.find(int(info[0])); + UASSERT(cter!=cellCount_.end()); + cter->second.second+=1; + } + else if(mapInfo.ptr(i, j-1)[0]>0.0f) + { + info[0] = mapInfo.ptr(i, j-1)[0]; + info[1] = float(j) * cellSize_ + xMin; + info[2] = float(i) * cellSize_ + yMin; + std::map >::iterator cter = cellCount_.find(int(info[0])); + UASSERT(cter!=cellCount_.end()); + cter->second.second+=1; + } + } + else + { + // fill empty + char sum = (map.at(i+1, j) == 0?1:0) + + (map.at(i-1, j) == 0?1:0) + + (map.at(i, j+1) == 0?1:0) + + (map.at(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(i+1, j) != -1 && mapInfo.ptr(i+1, j)[0]>0.0f) + { + info[0] = mapInfo.ptr(i+1, j)[0]; + info[1] = float(j) * cellSize_ + xMin; + info[2] = float(i) * cellSize_ + yMin; + std::map >::iterator cter = cellCount_.find(int(info[0])); + UASSERT(cter!=cellCount_.end()); + cter->second.first+=1; + } + else if(map.at(i-1, j) != -1 && mapInfo.ptr(i-1, j)[0]>0.0f) + { + info[0] = mapInfo.ptr(i-1, j)[0]; + info[1] = float(j) * cellSize_ + xMin; + info[2] = float(i) * cellSize_ + yMin; + std::map >::iterator cter = cellCount_.find(int(info[0])); + UASSERT(cter!=cellCount_.end()); + cter->second.first+=1; + } + } + } + } + + //float * info = mapInfo.ptr(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 & posesIn, float minMa } } - cache_.clear(); + if(!fullUpdate_) + { + cache_.clear(); + } UDEBUG("Occupancy Grid update time = %f s", timer.ticks()); } diff --git a/corelib/src/OctoMap.cpp b/corelib/src/OctoMap.cpp index 56cfd7c1..92b8d2ab 100644 --- a/corelib/src/OctoMap.cpp +++ b/corelib/src/OctoMap.cpp @@ -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 & poses) @@ -113,65 +114,77 @@ void OctoMap::update(const std::map & poses) if(graphChanged) { UINFO("Graph changed!"); - octomap::ColorOcTree * newOcTree = new octomap::ColorOcTree(octree_->getResolution()); - std::map newOccupiedCells; - int copied=0; - for(std::map::iterator iter = occupiedCells_.begin(); - iter!=occupiedCells_.end(); - ++iter) + if(fullUpdate_) { - std::map::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 newOccupiedCells; + int copied=0; + for(std::map::iterator iter = occupiedCells_.begin(); + iter!=occupiedCells_.end(); + ++iter) { - octomap::point3d pt = octree_->keyToCoord(iter->second.key_); - std::map::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::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::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 > orderedPoses; int lastId = addedNodes_.size()?addedNodes_.rbegin()->first:0; UDEBUG("Last id = %d", lastId); if(lastId >= 0) { - std::list > orderedPoses; for(std::map::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 & poses) break; } } + } - UDEBUG("orderedPoses = %d", (int)orderedPoses.size()); - for(std::list >::const_iterator iter=orderedPoses.begin(); iter!=orderedPoses.end(); ++iter) + UDEBUG("orderedPoses = %d", (int)orderedPoses.size()); + for(std::list >::const_iterator iter=orderedPoses.begin(); iter!=orderedPoses.end(); ++iter) + { + std::map::Ptr, pcl::PointCloud::Ptr> >::iterator cloudIter; + std::map >::iterator occupancyIter; + std::map::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::Ptr, pcl::PointCloud::Ptr> >::iterator cloudIter; - std::map >::iterator occupancyIter; - std::map::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; isecond.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; isecond.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::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; ifirst); + 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; isecond.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::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::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::Ptr ground(new pcl::PointCloud); pcl::PointCloud::Ptr obstacles(new pcl::PointCloud); @@ -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(0,0); for(unsigned int i=0;isize(); ++i) { - obstaclesMat.at(i)[0] = obstacles->at(i).x; - obstaclesMat.at(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(0,0); for(unsigned int i=0;isize(); ++i) { - groundMat.at(i)[0] = ground->at(i).x; - groundMat.at(i)[1] = ground->at(i).y; + ptr[i][0] = ground->at(i).x; + ptr[i][1] = ground->at(i).y; } std::map poses; diff --git a/corelib/src/util3d_mapping.cpp b/corelib/src/util3d_mapping.cpp index 0b4a1331..9983b4fc 100644 --- a/corelib/src/util3d_mapping.cpp +++ b/corelib/src/util3d_mapping.cpp @@ -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(i, j) == 100) + { + // remove obstacles which touch at least 3 empty cells but not unknown cells + int touchEmpty = (map.at(i+1, j) == 0?1:0) + + (map.at(i-1, j) == 0?1:0) + + (map.at(i, j+1) == 0?1:0) + + (map.at(i, j-1) == 0?1:0); + + if(touchEmpty>=3 && map.at(i+1, j) != -1 && + map.at(i-1, j) != -1 && + map.at(i, j+1) != -1 && + map.at(i, j-1) != -1) + { + erodedMap.at(i, j) = 0; // empty + } + } + } + } + return erodedMap; +} + } } diff --git a/guilib/include/rtabmap/gui/MainWindow.h b/guilib/include/rtabmap/gui/MainWindow.h index 3b45c7a0..c2676553 100644 --- a/guilib/include/rtabmap/gui/MainWindow.h +++ b/guilib/include/rtabmap/gui/MainWindow.h @@ -301,9 +301,6 @@ private: std::pair::Ptr, pcl::PointCloud::Ptr>, pcl::IndicesPtr> > _previousCloud; // used for subtraction std::map _createdScans; - std::map > _gridLocalMaps; // - std::map _gridViewPoints; - long _cachedGridsMemoryUsage; rtabmap::OccupancyGrid * _occupancyGrid; rtabmap::OctoMap * _octomap; diff --git a/guilib/include/rtabmap/gui/PreferencesDialog.h b/guilib/include/rtabmap/gui/PreferencesDialog.h index 9e599160..ebb4bb3b 100644 --- a/guilib/include/rtabmap/gui/PreferencesDialog.h +++ b/guilib/include/rtabmap/gui/PreferencesDialog.h @@ -169,7 +169,7 @@ public: bool isOctomapCubeRendering() const; bool isOctomap2dGrid() const; int getOctomapTreeDepth() const; - bool isOctomapGroundAnObstacle() const; + bool isOctomapFullUpdate() const; double getOctomapOccupancyThr() const; int getOctomapPointSize() const; int getCloudDecimation(int index) const; // 0=map, 1=odom @@ -199,7 +199,6 @@ public: bool getGridMapShown() const; double getGridMapResolution() const;; bool isGridMapEroded() const; - bool isGridMapIncremental() const; double getGridMapFootprintRadius() const; bool isGridMapFrom3DCloud() const; bool projMapFrame() const; diff --git a/guilib/src/DatabaseViewer.cpp b/guilib/src/DatabaseViewer.cpp index 3ccdc792..4fde7772 100644 --- a/guilib/src/DatabaseViewer.cpp +++ b/guilib/src/DatabaseViewer.cpp @@ -2673,6 +2673,10 @@ void DatabaseViewer::update(int value, localMaps, ui_->doubleSpinBox_gridCellSize->value(), xMin, yMin); + //OccupancyGrid grid(ui_->parameters_toolbox->getParameters()); + //grid.addToCache(data.id(), localMaps.begin()->second.first, localMaps.begin()->second.second); + //grid.update(poses); + //map8S = grid.getMap(xMin, yMin); } if(!map8S.empty()) { @@ -3892,7 +3896,7 @@ void DatabaseViewer::sliderIterationsValueChanged(int value) #ifdef RTABMAP_OCTOMAP if(ui_->checkBox_octomap->isChecked()) { - map = octomap_->createProjectionMap(xMin, yMin, cell, 0); + map = octomap_->createProjectionMap(xMin, yMin, cell, 0, ui_->spinBox_grid_depth->value()); } else #endif diff --git a/guilib/src/MainWindow.cpp b/guilib/src/MainWindow.cpp index 55c7a080..0a472200 100644 --- a/guilib/src/MainWindow.cpp +++ b/guilib/src/MainWindow.cpp @@ -154,7 +154,6 @@ MainWindow::MainWindow(PreferencesDialog * prefDialog, QWidget * parent) : _waypointsIndex(0), _cachedMemoryUsage(0), _createdCloudsMemoryUsage(0), - _cachedGridsMemoryUsage(0), _occupancyGrid(0), _octomap(0), _odometryCorrection(Transform::getIdentity()), @@ -246,7 +245,10 @@ MainWindow::MainWindow(PreferencesDialog * prefDialog, QWidget * parent) : _occupancyGrid = new OccupancyGrid(_preferencesDialog->getAllParameters()); #ifdef RTABMAP_OCTOMAP - _octomap = new OctoMap(_preferencesDialog->getGridMapResolution(), _preferencesDialog->getOctomapOccupancyThr()); + _octomap = new OctoMap( + _preferencesDialog->getGridMapResolution(), + _preferencesDialog->getOctomapOccupancyThr(), + _preferencesDialog->isOctomapFullUpdate()); #endif // Timer @@ -596,7 +598,6 @@ MainWindow::MainWindow(PreferencesDialog * prefDialog, QWidget * parent) : _ui->statsToolBox->updateStat("GUI/Refresh stats/ms", false); _ui->statsToolBox->updateStat("GUI/Cache Data Size/MB", false); _ui->statsToolBox->updateStat("GUI/Cache Clouds Size/MB", false); - _ui->statsToolBox->updateStat("GUI/Cache Grids Size/MB", false); #ifdef RTABMAP_OCTOMAP _ui->statsToolBox->updateStat("GUI/Octomap Size/MB", false); #endif @@ -1856,7 +1857,6 @@ void MainWindow::processStats(const rtabmap::Statistics & stat) } _ui->statsToolBox->updateStat("GUI/Cache Data Size/MB", _preferencesDialog->isTimeUsedInFigures()?stat.stamp()-_firstStamp:stat.refImageId(), _cachedMemoryUsage/(1024*1024), _preferencesDialog->isCacheSavedInFigures()); _ui->statsToolBox->updateStat("GUI/Cache Clouds Size/MB", _preferencesDialog->isTimeUsedInFigures()?stat.stamp()-_firstStamp:stat.refImageId(), _createdCloudsMemoryUsage/(1024*1024), _preferencesDialog->isCacheSavedInFigures()); - _ui->statsToolBox->updateStat("GUI/Cache Grids Size/MB", _preferencesDialog->isTimeUsedInFigures()?stat.stamp()-_firstStamp:stat.refImageId(), _cachedGridsMemoryUsage/(1024*1024), _preferencesDialog->isCacheSavedInFigures()); #ifdef RTABMAP_OCTOMAP _ui->statsToolBox->updateStat("GUI/Octomap Size/MB", _preferencesDialog->isTimeUsedInFigures()?stat.stamp()-_firstStamp:stat.refImageId(), _octomap->octree()->memoryUsage()/(1024*1024), _preferencesDialog->isCacheSavedInFigures()); #endif @@ -2052,16 +2052,10 @@ void MainWindow::updateMapCloud( } // occupancy grids - if(iter->first < 0) - { - _gridLocalMaps.erase(iter->first); - _gridViewPoints.erase(iter->first); - } - bool updateGridMap = ((_ui->graphicsView_graphView->isVisible() && _ui->graphicsView_graphView->isGridMapVisible()) || (_cloudViewer->isVisible() && _preferencesDialog->getGridMapShown())) && - _gridLocalMaps.find(iter->first) == _gridLocalMaps.end(); + _occupancyGrid->addedNodes().find(iter->first) == _occupancyGrid->addedNodes().end(); bool updateOctomap = false; #ifdef RTABMAP_OCTOMAP updateOctomap = @@ -2072,41 +2066,27 @@ void MainWindow::updateMapCloud( if(updateGridMap || updateOctomap) { QMap::iterator jter = _cachedSignatures.find(iter->first); - if(jter!=_cachedSignatures.end()) + if(jter!=_cachedSignatures.end() && jter->sensorData().gridCellSize() > 0.0f) { - if(_gridLocalMaps.find(iter->first) == _gridLocalMaps.end()) - { - cv::Mat ground; - cv::Mat obstacles; - if (jter->sensorData().gridCellSize() > 0.0f) - { - jter->sensorData().uncompressDataConst(0, 0, 0, 0, &ground, &obstacles); - _gridLocalMaps.insert(std::make_pair(iter->first, std::make_pair(ground, obstacles))); - _gridViewPoints.insert(std::make_pair(iter->first, jter->sensorData().gridViewPoint())); - _cachedGridsMemoryUsage += (long)(ground.total()*ground.elemSize() + obstacles.total()*obstacles.elemSize()); + cv::Mat ground; + cv::Mat obstacles; + + jter->sensorData().uncompressDataConst(0, 0, 0, 0, &ground, &obstacles); + + _occupancyGrid->addToCache(iter->first, ground, obstacles); - if (ground.cols || obstacles.cols) - { - _occupancyGrid->addToCache(iter->first, ground, obstacles); - } - } - } #ifdef RTABMAP_OCTOMAP if(updateOctomap) { - std::map >::iterator mter = _gridLocalMaps.find(iter->first); - std::map::iterator pter = _gridViewPoints.find(iter->first); - if(mter != _gridLocalMaps.end() && pter!=_gridViewPoints.end()) + if((ground.empty() || ground.channels() > 2) && + (obstacles.empty() || obstacles.channels() > 2)) { - if((mter->second.first.empty() || mter->second.first.channels() > 2) && - (mter->second.second.empty() || mter->second.second.channels() > 2)) - { - _octomap->addToCache(iter->first, mter->second.first, mter->second.second, pter->second); - } - else if(!mter->second.first.empty() && !mter->second.second.empty()) - { - UWARN("Node %d: Cannot update octomap with 2D occupancy grids.", iter->first); - } + cv::Point3f viewpoint = jter->sensorData().gridViewPoint(); + _octomap->addToCache(iter->first, ground, obstacles, viewpoint); + } + else if(!ground.empty() || !obstacles.empty()) + { + UWARN("Node %d: Cannot update octomap with 2D occupancy grids.", iter->first); } } #endif @@ -2384,8 +2364,7 @@ void MainWindow::updateMapCloud( _ui->graphicsView_graphView->updateGTGraph(_currentGTPosesMap); } cv::Mat map8U; - if((_ui->graphicsView_graphView->isVisible() || _preferencesDialog->getGridMapShown()) && - _gridLocalMaps.size()) + if((_ui->graphicsView_graphView->isVisible() || _preferencesDialog->getGridMapShown())) { float xMin, yMin; float resolution = _preferencesDialog->getGridMapResolution(); @@ -2393,35 +2372,26 @@ void MainWindow::updateMapCloud( #ifdef RTABMAP_OCTOMAP if(_preferencesDialog->isOctomap2dGrid()) { - map8S = _octomap->createProjectionMap(xMin, yMin, resolution, 0); + map8S = _octomap->createProjectionMap(xMin, yMin, resolution, 0, _preferencesDialog->getOctomapTreeDepth()); } else #endif { - if(_preferencesDialog->isGridMapIncremental()) + _occupancyGrid->update(poses, 0, _preferencesDialog->getGridMapFootprintRadius()); + if(stats) { - _occupancyGrid->update(poses, 0, _preferencesDialog->getGridMapFootprintRadius()); - if(stats) - { - stats->insert(std::make_pair("GUI/Grid Update/ms", (float)timer.restart()*1000.0f)); - } - map8S = _occupancyGrid->getMap(xMin, yMin); - } - else - { - map8S = util3d::create2DMapFromOccupancyLocalMaps( - poses, - _gridLocalMaps, - resolution, - xMin, yMin, - 0, - _preferencesDialog->isGridMapEroded(), - _preferencesDialog->getGridMapFootprintRadius()); + stats->insert(std::make_pair("GUI/Grid Update/ms", (float)timer.restart()*1000.0f)); } + map8S = _occupancyGrid->getMap(xMin, yMin); } if(!map8S.empty()) { + if(_preferencesDialog->isGridMapEroded()) + { + map8S = util3d::erodeMap(map8S); + } + //convert to gray scaled map map8U = util3d::convertMap2Image8U(map8S); @@ -2516,7 +2486,7 @@ void MainWindow::updateMapCloud( _ui->actionSave_point_cloud->setEnabled(!_cachedSignatures.empty() || !_cachedClouds.empty()); _ui->actionView_high_res_point_cloud->setEnabled(!_cachedSignatures.empty() || !_cachedClouds.empty()); _ui->actionExport_2D_scans_ply_pcd->setEnabled(!_createdScans.empty()); - _ui->actionExport_2D_Grid_map_bmp_png->setEnabled(!_gridLocalMaps.empty()); + _ui->actionExport_2D_Grid_map_bmp_png->setEnabled(!_occupancyGrid->addedNodes().empty()); _ui->actionView_scans->setEnabled(!_createdScans.empty()); #ifdef RTABMAP_OCTOMAP _ui->actionExport_octomap->setEnabled(_octomap->octree()->size()); @@ -4477,17 +4447,18 @@ void MainWindow::startDetection() "progress will not be shown in the GUI.")); } + _occupancyGrid->clear(); + _occupancyGrid->parseParameters(parameters); + #ifdef RTABMAP_OCTOMAP UASSERT(_octomap != 0); delete _octomap; _octomap = new OctoMap( _preferencesDialog->getGridMapResolution(), - _preferencesDialog->getOctomapOccupancyThr()); + _preferencesDialog->getOctomapOccupancyThr(), + _preferencesDialog->isOctomapFullUpdate()); #endif - _occupancyGrid->clear(); - _occupancyGrid->parseParameters(parameters); - // clear odometry visual stuff _cloudViewer->removeCloud("cloudOdom"); _cloudViewer->removeCloud("scanOdom"); @@ -5614,8 +5585,6 @@ void MainWindow::clearTheCache() _previousCloud.second.first.second.reset(); _previousCloud.second.second.reset(); _createdScans.clear(); - _gridLocalMaps.clear(); - _cachedGridsMemoryUsage = 0; _createdFeatures.clear(); _cloudViewer->clear(); _cloudViewer->setBackgroundColor(_cloudViewer->getDefaultBackgroundColor()); @@ -5664,7 +5633,10 @@ void MainWindow::clearTheCache() // re-create one if the resolution has changed UASSERT(_octomap != 0); delete _octomap; - _octomap = new OctoMap(_preferencesDialog->getGridMapResolution(), _preferencesDialog->getOctomapOccupancyThr()); + _octomap = new OctoMap( + _preferencesDialog->getGridMapResolution(), + _preferencesDialog->getOctomapOccupancyThr(), + _preferencesDialog->isOctomapFullUpdate()); #endif _occupancyGrid->clear(); } @@ -5944,17 +5916,16 @@ void MainWindow::exportGridMap() else #endif { - pixels = util3d::create2DMapFromOccupancyLocalMaps( - poses, - _gridLocalMaps, - gridCellSize, - xMin, yMin, - 0, - _preferencesDialog->isGridMapEroded()); + pixels = _occupancyGrid->getMap(xMin, yMin); } if(!pixels.empty()) { + if(_preferencesDialog->isGridMapEroded()) + { + pixels = util3d::erodeMap(pixels); + } + cv::Mat map8U(pixels.rows, pixels.cols, CV_8U); //convert to gray scaled map for (int i = 0; i < pixels.rows; ++i) @@ -6617,7 +6588,7 @@ void MainWindow::changeState(MainWindow::State newState) _ui->actionSave_point_cloud->setEnabled(!_cachedSignatures.empty() || !_cachedClouds.empty()); _ui->actionView_high_res_point_cloud->setEnabled(!_cachedSignatures.empty() || !_cachedClouds.empty()); _ui->actionExport_2D_scans_ply_pcd->setEnabled(!_createdScans.empty()); - _ui->actionExport_2D_Grid_map_bmp_png->setEnabled(!_gridLocalMaps.empty()); + _ui->actionExport_2D_Grid_map_bmp_png->setEnabled(!_occupancyGrid->addedNodes().empty()); _ui->actionView_scans->setEnabled(!_createdScans.empty()); #ifdef RTABMAP_OCTOMAP _ui->actionExport_octomap->setEnabled(_octomap->octree()->size()); @@ -6679,7 +6650,7 @@ void MainWindow::changeState(MainWindow::State newState) _ui->actionSave_point_cloud->setEnabled(!_cachedSignatures.empty() || !_cachedClouds.empty()); _ui->actionView_high_res_point_cloud->setEnabled(!_cachedSignatures.empty() || !_cachedClouds.empty()); _ui->actionExport_2D_scans_ply_pcd->setEnabled(!_createdScans.empty()); - _ui->actionExport_2D_Grid_map_bmp_png->setEnabled(!_gridLocalMaps.empty()); + _ui->actionExport_2D_Grid_map_bmp_png->setEnabled(!_occupancyGrid->addedNodes().empty()); _ui->actionView_scans->setEnabled(!_createdScans.empty()); #ifdef RTABMAP_OCTOMAP _ui->actionExport_octomap->setEnabled(_octomap->octree()->size()); @@ -6805,7 +6776,7 @@ void MainWindow::changeState(MainWindow::State newState) _ui->actionSave_point_cloud->setEnabled(!_cachedSignatures.empty() || !_cachedClouds.empty()); _ui->actionView_high_res_point_cloud->setEnabled(!_cachedSignatures.empty() || !_cachedClouds.empty()); _ui->actionExport_2D_scans_ply_pcd->setEnabled(!_createdScans.empty()); - _ui->actionExport_2D_Grid_map_bmp_png->setEnabled(!_gridLocalMaps.empty()); + _ui->actionExport_2D_Grid_map_bmp_png->setEnabled(!_occupancyGrid->addedNodes().empty()); _ui->actionView_scans->setEnabled(!_createdScans.empty()); #ifdef RTABMAP_OCTOMAP _ui->actionExport_octomap->setEnabled(_octomap->octree()->size()); @@ -6872,7 +6843,7 @@ void MainWindow::changeState(MainWindow::State newState) _ui->actionSave_point_cloud->setEnabled(!_cachedSignatures.empty() || !_cachedClouds.empty()); _ui->actionView_high_res_point_cloud->setEnabled(!_cachedSignatures.empty() || !_cachedClouds.empty()); _ui->actionExport_2D_scans_ply_pcd->setEnabled(!_createdScans.empty()); - _ui->actionExport_2D_Grid_map_bmp_png->setEnabled(!_gridLocalMaps.empty()); + _ui->actionExport_2D_Grid_map_bmp_png->setEnabled(!_occupancyGrid->addedNodes().empty()); _ui->actionView_scans->setEnabled(!_createdScans.empty()); #ifdef RTABMAP_OCTOMAP _ui->actionExport_octomap->setEnabled(_octomap->octree()->size()); diff --git a/guilib/src/PreferencesDialog.cpp b/guilib/src/PreferencesDialog.cpp index cb7462c7..3b50da8e 100644 --- a/guilib/src/PreferencesDialog.cpp +++ b/guilib/src/PreferencesDialog.cpp @@ -409,6 +409,7 @@ PreferencesDialog::PreferencesDialog(QWidget * parent) : connect(_ui->groupBox_octomap, SIGNAL(toggled(bool)), this, SLOT(makeObsoleteCloudRenderingPanel())); connect(_ui->spinBox_octomap_treeDepth, SIGNAL(valueChanged(int)), this, SLOT(makeObsoleteCloudRenderingPanel())); + connect(_ui->checkBox_octomap_fullUpdate, SIGNAL(stateChanged(int)), this, SLOT(makeObsoleteCloudRenderingPanel())); connect(_ui->checkBox_octomap_2dgrid, SIGNAL(stateChanged(int)), this, SLOT(makeObsoleteCloudRenderingPanel())); connect(_ui->checkBox_octomap_show3dMap, SIGNAL(stateChanged(int)), this, SLOT(makeObsoleteCloudRenderingPanel())); connect(_ui->checkBox_octomap_cubeRendering, SIGNAL(stateChanged(int)), this, SLOT(makeObsoleteCloudRenderingPanel())); @@ -842,6 +843,7 @@ PreferencesDialog::PreferencesDialog(QWidget * parent) : _ui->checkBox_grid_flatObstaclesDetected->setObjectName(Parameters::kGridFlatObstacleDetected().c_str()); _ui->groupBox_grid_fromDepthImage->setObjectName(Parameters::kGridFromDepth().c_str()); _ui->checkBox_grid_projMapFrame->setObjectName(Parameters::kGridMapFrameProjection().c_str()); + _ui->checkBox_grid_fullUpdate->setObjectName(Parameters::kGridFullUpdate().c_str()); _ui->doubleSpinBox_grid_maxGroundAngle->setObjectName(Parameters::kGridMaxGroundAngle().c_str()); _ui->spinBox_grid_normalK->setObjectName(Parameters::kGridNormalK().c_str()); _ui->doubleSpinBox_grid_maxGroundHeight->setObjectName(Parameters::kGridMaxGroundHeight().c_str()); @@ -1306,15 +1308,15 @@ void PreferencesDialog::resetSettings(QGroupBox * groupBox) _ui->checkBox_map_shown->setChecked(false); _ui->doubleSpinBox_map_resolution->setValue(0.05); _ui->checkBox_map_erode->setChecked(false); - _ui->checkBox_map_incremental->setChecked(false); _ui->doubleSpinBox_map_footprintRadius->setValue(0); _ui->doubleSpinBox_map_opacity->setValue(0.75); _ui->groupBox_octomap->setChecked(false); _ui->spinBox_octomap_treeDepth->setValue(16); + _ui->checkBox_octomap_fullUpdate->setChecked(false); _ui->checkBox_octomap_2dgrid->setChecked(true); _ui->checkBox_octomap_show3dMap->setChecked(true); - _ui->checkBox_octomap_cubeRendering->setChecked(true); + _ui->checkBox_octomap_cubeRendering->setChecked(false); _ui->spinBox_octomap_pointSize->setValue(5); _ui->doubleSpinBox_octomap_occupancyThr->setValue(0.5); } @@ -1685,12 +1687,12 @@ void PreferencesDialog::readGuiSettings(const QString & filePath) _ui->checkBox_map_shown->setChecked(settings.value("gridMapShown", _ui->checkBox_map_shown->isChecked()).toBool()); _ui->doubleSpinBox_map_resolution->setValue(settings.value("gridMapResolution", _ui->doubleSpinBox_map_resolution->value()).toDouble()); _ui->checkBox_map_erode->setChecked(settings.value("gridMapEroded", _ui->checkBox_map_erode->isChecked()).toBool()); - _ui->checkBox_map_incremental->setChecked(settings.value("gridMapIncremental", _ui->checkBox_map_incremental->isChecked()).toBool()); _ui->doubleSpinBox_map_footprintRadius->setValue(settings.value("gridMapFootprintRadius", _ui->doubleSpinBox_map_footprintRadius->value()).toDouble()); _ui->doubleSpinBox_map_opacity->setValue(settings.value("gridMapOpacity", _ui->doubleSpinBox_map_opacity->value()).toDouble()); _ui->groupBox_octomap->setChecked(settings.value("octomap", _ui->groupBox_octomap->isChecked()).toBool()); _ui->spinBox_octomap_treeDepth->setValue(settings.value("octomap_depth", _ui->spinBox_octomap_treeDepth->value()).toInt()); + _ui->checkBox_octomap_fullUpdate->setChecked(settings.value("octomap_full_update", _ui->checkBox_octomap_fullUpdate->isChecked()).toBool()); _ui->checkBox_octomap_2dgrid->setChecked(settings.value("octomap_2dgrid", _ui->checkBox_octomap_2dgrid->isChecked()).toBool()); _ui->checkBox_octomap_show3dMap->setChecked(settings.value("octomap_3dmap", _ui->checkBox_octomap_show3dMap->isChecked()).toBool()); _ui->checkBox_octomap_cubeRendering->setChecked(settings.value("octomap_cube", _ui->checkBox_octomap_cubeRendering->isChecked()).toBool()); @@ -2071,12 +2073,12 @@ void PreferencesDialog::writeGuiSettings(const QString & filePath) const settings.setValue("gridMapShown", _ui->checkBox_map_shown->isChecked()); settings.setValue("gridMapResolution", _ui->doubleSpinBox_map_resolution->value()); settings.setValue("gridMapEroded", _ui->checkBox_map_erode->isChecked()); - settings.setValue("gridMapIncremental", _ui->checkBox_map_incremental->isChecked()); settings.setValue("gridMapFootprintRadius", _ui->doubleSpinBox_map_footprintRadius->value()); settings.setValue("gridMapOpacity", _ui->doubleSpinBox_map_opacity->value()); settings.setValue("octomap", _ui->groupBox_octomap->isChecked()); settings.setValue("octomap_depth", _ui->spinBox_octomap_treeDepth->value()); + settings.setValue("octomap_full_update", _ui->checkBox_octomap_fullUpdate->isChecked()); settings.setValue("octomap_2dgrid", _ui->checkBox_octomap_2dgrid->isChecked()); settings.setValue("octomap_3dmap", _ui->checkBox_octomap_show3dMap->isChecked()); settings.setValue("octomap_cube", _ui->checkBox_octomap_cubeRendering->isChecked()); @@ -4064,9 +4066,9 @@ int PreferencesDialog::getOctomapTreeDepth() const { return _ui->spinBox_octomap_treeDepth->value(); } -bool PreferencesDialog::isOctomapGroundAnObstacle() const +bool PreferencesDialog::isOctomapFullUpdate() const { - return _ui->checkBox_grid_groundObstacle->isChecked(); + return _ui->checkBox_octomap_fullUpdate->isChecked(); } double PreferencesDialog::getOctomapOccupancyThr() const { @@ -4255,10 +4257,6 @@ bool PreferencesDialog::isGridMapEroded() const { return _ui->checkBox_map_erode->isChecked(); } -bool PreferencesDialog::isGridMapIncremental() const -{ - return _ui->checkBox_map_incremental->isChecked(); -} double PreferencesDialog::getGridMapFootprintRadius() const { return _ui->doubleSpinBox_map_footprintRadius->value(); diff --git a/guilib/src/ui/preferencesDialog.ui b/guilib/src/ui/preferencesDialog.ui index d703c936..f5ac8c3e 100644 --- a/guilib/src/ui/preferencesDialog.ui +++ b/guilib/src/ui/preferencesDialog.ui @@ -65,7 +65,7 @@ 0 0 673 - 2718 + 2735 @@ -1794,6 +1794,26 @@ Show a yellow background when the number of odometry inliers goes under this thr + + + + Resolution (cell size). + + + Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse + + + + + + + Opacity. + + + Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse + + + @@ -1849,30 +1869,10 @@ Show a yellow background when the number of odometry inliers goes under this thr - - - - Resolution (cell size). - - - Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse - - - - - - - Opacity. - - - Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse - - - - Erode. Not used if incremental. + Erode. true @@ -1924,29 +1924,6 @@ Show a yellow background when the number of odometry inliers goes under this thr - - - - Incremental. - - - true - - - Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse - - - - - - - - - - false - - - @@ -1958,7 +1935,17 @@ Show a yellow background when the number of odometry inliers goes under this thr true - + + + + + + + false + + + + Occupancy threshold. @@ -1971,20 +1958,20 @@ Show a yellow background when the number of odometry inliers goes under this thr - - - - Cube rendering. Disable to show as a point cloud (a lot less GPU power required). + + + + 1.000000000000000 - - true + + 0.050000000000000 - - Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse + + 0.500000000000000 - + Octomap maximum tree depth (max 16). The highest depth means the smallest resolution of the map (cell size). At smallest resolution the octomap shows RGB colors. Other resolutions produce z-axis gradient colored octomap. @@ -1997,7 +1984,7 @@ Show a yellow background when the number of odometry inliers goes under this thr - + 1 @@ -2010,8 +1997,34 @@ Show a yellow background when the number of odometry inliers goes under this thr + + + + 1 + + + 99 + + + 5 + + + + + + + Full update. When the graph is changed, the whole map will be reconstructed instead of moving individually each cells of the map. Also, data added to cache won't be released after updating the map. This process is longer but more robust to drift that would erase some parts of the map when it should not. + + + true + + + Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse + + + - + @@ -2020,10 +2033,10 @@ Show a yellow background when the number of odometry inliers goes under this thr - - + + - Show 2D occupancy grid map from OctoMap projection. + Cube rendering. Warning: this requires significant more GPU power. true @@ -2046,30 +2059,7 @@ Show a yellow background when the number of odometry inliers goes under this thr - - - - - - - false - - - - - - - 1.000000000000000 - - - 0.050000000000000 - - - 0.500000000000000 - - - - + @@ -2079,7 +2069,7 @@ Show a yellow background when the number of odometry inliers goes under this thr - + Point size. When cube rendering is disabled. @@ -2092,16 +2082,26 @@ Show a yellow background when the number of odometry inliers goes under this thr - - - - 1 + + + + 2D grid map created from OctoMap. - - 99 + + true - - 5 + + Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse + + + + + + + + + + false @@ -8421,43 +8421,7 @@ If set to false, classic RTAB-Map loop closure detection is done using only imag - - - - Minimum ground height (0=disabled). - - - true - - - Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse - - - - - - - Resolution (cell size). - - - Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse - - - - - - - Footprint filtering height (0=disabled). Footprint length and width should be set. - - - true - - - Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse - - - - + m @@ -8476,111 +8440,7 @@ If set to false, classic RTAB-Map loop closure detection is done using only imag - - - - m - - - 2 - - - 9999.000000000000000 - - - - - - - m - - - 2 - - - 99999.000000000000000 - - - 0.010000000000000 - - - - - - - Noise filtering min neighbors. - - - true - - - Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse - - - - - - - Noise filtering radius (0=disabled). Done after segmentation. - - - true - - - Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse - - - - - - - - - - false - - - - - - - Projection in map frame. On a 3D terrain and a fixed local camera transform (the cloud is created relative to ground), you may want to disable this to do the projection in robot frame instead. - - - true - - - Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse - - - - - - - Maximum ground height (0=disabled). Should be set if Normals Segmentation Approach is checked below. - - - true - - - Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse - - - - - - - Maximum obstacles height (0=disabled). - - - true - - - Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse - - - - + m @@ -8602,7 +8462,88 @@ If set to false, classic RTAB-Map loop closure detection is done using only imag + + + + Footprint filtering height (0=disabled). Footprint length and width should be set. + + + true + + + Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse + + + + + + + m + + + 2 + + + 9999.000000000000000 + + + + + + m + + + 2 + + + 99999.000000000000000 + + + 0.010000000000000 + + + + + + + Noise filtering min neighbors. + + + true + + + Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse + + + + + + + Maximum ground height (0=disabled). Should be set if Normals Segmentation Approach is checked below. + + + true + + + Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse + + + + + + + Maximum obstacles height (0=disabled). + + + true + + + Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse + + + + 1 @@ -8615,7 +8556,7 @@ If set to false, classic RTAB-Map loop closure detection is done using only imag - + m @@ -8631,7 +8572,7 @@ If set to false, classic RTAB-Map loop closure detection is done using only imag - + Laser scan decimation. @@ -8644,7 +8585,7 @@ If set to false, classic RTAB-Map loop closure detection is done using only imag - + 1 @@ -8657,7 +8598,7 @@ If set to false, classic RTAB-Map loop closure detection is done using only imag - + Footprint filtering length (0=disabled). @@ -8670,7 +8611,7 @@ If set to false, classic RTAB-Map loop closure detection is done using only imag - + Footprint filtering width (0=disabled). Footprint length should be set. @@ -8683,26 +8624,7 @@ If set to false, classic RTAB-Map loop closure detection is done using only imag - - - - m - - - 2 - - - 10.000000000000000 - - - 0.100000000000000 - - - 0.000000000000000 - - - - + m @@ -8721,7 +8643,7 @@ If set to false, classic RTAB-Map loop closure detection is done using only imag - + m @@ -8740,6 +8662,74 @@ If set to false, classic RTAB-Map loop closure detection is done using only imag + + + + Global occupancy grid full update. When the graph is changed, the whole map will be reconstructed instead of moving individually each cells of the map. Also, data added to cache won't be released after updating the map. This process is longer but more robust to drift that would erase some parts of the map when it should not. + + + true + + + Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse + + + + + + + Minimum ground height (0=disabled). + + + true + + + Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse + + + + + + + Resolution (cell size). + + + Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse + + + + + + + Noise filtering radius (0=disabled). Done after segmentation. + + + true + + + Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse + + + + + + + m + + + 2 + + + 10.000000000000000 + + + 0.100000000000000 + + + 0.000000000000000 + + + @@ -8763,6 +8753,39 @@ If set to false, classic RTAB-Map loop closure detection is done using only imag + + + + + + + false + + + + + + + Projection in map frame. On a 3D terrain and a fixed local camera transform (the cloud is created relative to ground), you may want to disable this to do the projection in robot frame instead. + + + true + + + Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse + + + + + + + + + + false + + +