diff --git a/corelib/src/Rtabmap.cpp b/corelib/src/Rtabmap.cpp index 2911e527..aa9f7d10 100644 --- a/corelib/src/Rtabmap.cpp +++ b/corelib/src/Rtabmap.cpp @@ -1334,6 +1334,7 @@ bool Rtabmap::process( // When analysing logs, it's convenient to know // if the hypothesis would be rejected if T_loop would be lower. rejectedHypothesis = true; + UWARN("rejected hypothesis: under loop ratio %f < %f", _highestHypothesis.second, _loopRatio*lastHighestHypothesis.second); } //for statistic... @@ -1791,7 +1792,7 @@ bool Rtabmap::process( rejectedHypothesis = transform.isNull(); if(rejectedHypothesis) { - UINFO("Rejected loop closure %d -> %d: %s", + UWARN("Rejected loop closure %d -> %d: %s", _loopClosureHypothesis.first, signature->id(), rejectedMsg.c_str()); } } diff --git a/corelib/src/util3d_mapping.cpp b/corelib/src/util3d_mapping.cpp index 62bff5cb..74f8de58 100644 --- a/corelib/src/util3d_mapping.cpp +++ b/corelib/src/util3d_mapping.cpp @@ -349,7 +349,7 @@ cv::Mat create2DMapFromOccupancyLocalMaps( if(erode) { - // remove obstacles which touch to empty cells but not unknown cells + // remove obstacles which touch at least 3 empty cells but not unknown cells cv::Mat erodedMap = map.clone(); for(std::list >::iterator iter = obstacleIndices.begin(); iter!= obstacleIndices.end(); @@ -357,11 +357,11 @@ cv::Mat create2DMapFromOccupancyLocalMaps( { int i = iter->first; int j = iter->second; - bool touchEmpty = map.at(i+1, j) == 0 || - map.at(i-1, j) == 0 || - map.at(i, j+1) == 0 || - map.at(i, j-1) == 0; - if(touchEmpty && map.at(i+1, j) != -1 && + 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)