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

improve flood fill and move it to update function (#727)

Browse Source 
* add flood fill filter

* change to use unordoned_multiset

* remove recursivity in flood fill and move the flood fill into the update

* Refactoring of OctoMap constructors (keeping only one). Simplified floodfill parameters to only one and added it to UI.

* Fixed ray tracing not done in local 3D maps (OctoMap)

Co-authored-by: MarcLeclercGit <marcantoine.leclerc96@gmail.com>
Co-authored-by: matlabbe <matlabbe@gmail.com>
This commit is contained in:
chameau5050
2021-05-30 10:12:07 -04:00
committed by GitHub
parent b7c2f6801b
commit f94f9cbb9b
7 changed files with 431 additions and 321 deletions
Download Patch File Download Diff File Expand all files Collapse all files

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

@@ -40,7 +40,9 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
#include <rtabmap/core/Parameters.h>
#include <map>
#include <unordered_set>
#include <string>
#include <queue>
namespace rtabmap {
@@ -169,8 +171,7 @@ class RtabmapColorOcTree : public octomap::OccupancyOcTreeBase <RtabmapColorOcTr
class RTABMAP_EXP OctoMap {
public:
OctoMap(const ParametersMap & parameters);
OctoMap(float cellSize = 0.1f, float occupancyThr = 0.5f, bool fullUpdate = false, float updateError=0.01f);
OctoMap(const ParametersMap & parameters = ParametersMap());
const std::map<int, Transform> & addedNodes() const {return addedNodes_;}
void addToCache(int nodeId,
@@ -193,8 +194,7 @@ public:
std::vector<int> * groundIndices = 0,
bool originalRefPoints = true,
std::vector<int> * frontierIndices = 0,
std::vector<double> * cloudProb = 0,
bool applyFloodFill = false ) const;
std::vector<double> * cloudProb = 0) const;
cv::Mat createProjectionMap(
float & xMin,
@@ -215,6 +215,12 @@ public:
void setRayTracing(bool enabled) {rayTracing_ = enabled;}
bool hasColor() const {return hasColor_;}
static std::unordered_set<octomap::OcTreeKey, octomap::OcTreeKey::KeyHash> findEmptyNode(RtabmapColorOcTree* octree_, unsigned int treeDepth, octomap::point3d startPosition);
static void floodFill(RtabmapColorOcTree* octree_, unsigned int treeDepth,octomap::point3d startPosition, std::unordered_set<octomap::OcTreeKey, octomap::OcTreeKey::KeyHash> & EmptyNodes,std::queue<octomap::point3d>& positionToExplore);
static bool isNodeVisited(std::unordered_set<octomap::OcTreeKey,octomap::OcTreeKey::KeyHash> const & EmptyNodes,octomap::OcTreeKey const key);
static octomap::point3d findCloseEmpty(RtabmapColorOcTree* octree_, unsigned int treeDepth,octomap::point3d startPosition);
static bool isValidEmpty(RtabmapColorOcTree* octree_, unsigned int treeDepth,octomap::point3d startPosition);
private:
void updateMinMax(const octomap::point3d & point);
@@ -229,6 +235,7 @@ private:
float updateError_;
float rangeMax_;
bool rayTracing_;
unsigned int emptyFloodFillDepth_;
double minValues_[3];
double maxValues_[3];
};

1
corelib/include/rtabmap/core/Parameters.h
View File

@@ -764,6 +764,7 @@ class RTABMAP_EXP Parameters
RTABMAP_PARAM(GridGlobal, ProbMiss, float, 0.4, "Probability of a miss (value between 0 and 0.5).");
RTABMAP_PARAM(GridGlobal, ProbClampingMin, float, 0.1192, "Probability clamping minimum (value between 0 and 1).");
RTABMAP_PARAM(GridGlobal, ProbClampingMax, float, 0.971, "Probability clamping maximum (value between 0 and 1).");
RTABMAP_PARAM(GridGlobal, FloodFillDepth, unsigned int, 0, "Flood fill filter (0=disabled), used to remove empty cells outside the map. The flood fill is done at the specified depth (between 1 and 16) of the OctoMap.");
RTABMAP_PARAM(Marker, Dictionary, int, 0, "Dictionary to use: DICT_ARUCO_4X4_50=0, DICT_ARUCO_4X4_100=1, DICT_ARUCO_4X4_250=2, DICT_ARUCO_4X4_1000=3, DICT_ARUCO_5X5_50=4, DICT_ARUCO_5X5_100=5, DICT_ARUCO_5X5_250=6, DICT_ARUCO_5X5_1000=7, DICT_ARUCO_6X6_50=8, DICT_ARUCO_6X6_100=9, DICT_ARUCO_6X6_250=10, DICT_ARUCO_6X6_1000=11, DICT_ARUCO_7X7_50=12, DICT_ARUCO_7X7_100=13, DICT_ARUCO_7X7_250=14, DICT_ARUCO_7X7_1000=15, DICT_ARUCO_ORIGINAL = 16, DICT_APRILTAG_16h5=17, DICT_APRILTAG_25h9=18, DICT_APRILTAG_36h10=19, DICT_APRILTAG_36h11=20");
RTABMAP_PARAM(Marker, Length, float, 0, "The length (m) of the markers' side. 0 means automatic marker length estimation using the depth image (the camera should look at the marker perpendicularly for initialization).");