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

Updated how local scan matching is done (doing it on segmented local paths)

Browse Source
This commit is contained in:
Mathieu Labbe
2015-04-27 02:43:06 -04:00
parent 2e706ed01f
commit db90da7303
16 changed files with 532 additions and 226 deletions
Download Patch File Download Diff File Expand all files Collapse all files

4
corelib/include/rtabmap/core/DBReader.h
View File

@@ -49,7 +49,7 @@ public:
DBReader(const std::string & databasePath,
float frameRate = 0.0f,
bool odometryIgnored = false,
float delayToStartSec = 0.0f);
bool ignoreGoalDelay = false);
virtual ~DBReader();
bool init(int startIndex=0);
@@ -64,7 +64,7 @@ private:
std::string _path;
float _frameRate;
bool _odometryIgnored;
float _delayToStartSec;
bool _ignoreGoalDelay;
DBDriver * _dbDriver;
UTimer _timer;

4
corelib/include/rtabmap/core/Graph.h
View File

@@ -145,6 +145,10 @@ std::multimap<int, Link>::iterator RTABMAP_EXP findLink(
std::multimap<int, Link> & links,
int from,
int to);
std::multimap<int, int>::iterator RTABMAP_EXP findLink(
std::multimap<int, int> & links,
int from,
int to);
/**
* Get only the the most recent or older poses in the defined radius.

3
corelib/include/rtabmap/core/Memory.h
View File

@@ -220,6 +220,7 @@ private:
bool _generateIds;
bool _badSignaturesIgnored;
int _imageDecimation;
float _laserScanVoxelSize;
bool _localSpaceLinksKeptInWM;
float _rehearsalMaxDistance;
float _rehearsalMaxAngle;
@@ -255,6 +256,8 @@ private:
bool _bowForce2D;
bool _bowEpipolarGeometry;
float _bowEpipolarGeometryVar;
float _icpMaxTranslation;
float _icpMaxRotation;
int _icpDecimation;
float _icpMaxDepth;
float _icpVoxelSize;

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

@@ -194,7 +194,8 @@ class RTABMAP_EXP Parameters
RTABMAP_PARAM(Mem, GenerateIds, bool, true, "True=Generate location IDs, False=use input image IDs.");
RTABMAP_PARAM(Mem, BadSignaturesIgnored, bool, false, "Bad signatures are ignored.");
RTABMAP_PARAM(Mem, InitWMWithAllNodes, bool, false, "Initialize the Working Memory with all nodes in Long-Term Memory. When false, it is initialized with nodes of the previous session.");
RTABMAP_PARAM(Mem, ImageDecimation, int, 1, "Image decimation (>=1).");
RTABMAP_PARAM(Mem, ImageDecimation, int, 1, "Image decimation (>=1) when creating a signature.");
RTABMAP_PARAM(Mem, LaserScanVoxelSize, float, 0.0, "If > 0.0, voxelize laser scans when creating a signature.");
RTABMAP_PARAM(Mem, LocalSpaceLinksKeptInWM, bool, true, "If local space links are kept in WM.");
@@ -287,7 +288,8 @@ class RTABMAP_EXP Parameters
RTABMAP_PARAM(RGBD, NewMapOdomChangeDistance, float, 0, "A new map is created if a change of odometry translation greater than X m is detected (0 m = disabled).");
RTABMAP_PARAM(RGBD, OptimizeFromGraphEnd, bool, false, "Optimize graph from the newest node. If false, the graph is optimized from the oldest node of the current graph (this adds an overhead computation to detect to oldest mode of the current graph, but it can be useful to preserve the map referential from the oldest node). Warning when set to false: when some nodes are transferred, the first referential of the local map may change, resulting in momentary changes in robot/map position (which are annoying in teleoperation).");
RTABMAP_PARAM(RGBD, GoalReachedRadius, float, 0.5, "Goal reached radius (m).");
RTABMAP_PARAM(RGBD, PlanWithNearNodesLinked, bool, true, "Before planning in the graph, near nodes are linked together (even if they don't belong to same map). Radius is defined by \"RGBD/GoalReachedRadius\" parameter.");
RTABMAP_PARAM(RGBD, PlanVirtualLinks, bool, true, "Before planning in the graph, close nodes are linked together. Radius is defined by \"RGBD/GoalReachedRadius\" parameter.");
RTABMAP_PARAM(RGBD, PlanVirtualLinksMaxDiffID, int, 50, "Max difference ID to add virtual links before planning.");
RTABMAP_PARAM(RGBD, GoalsSavedInUserData, bool, true, "When a goal is received and processed with success, it is saved in user data of the location with this format: \"GOAL:#\".");
RTABMAP_PARAM(RGBD, MaxLocalRetrieved, unsigned int, 2, "Maximum local locations retrieved (0=disabled) near the current pose in the local map or on the current planned path (those on the planned path have priority).");
RTABMAP_PARAM(RGBD, LocalRadius, float, 10, "Local radius (m) for nodes selection in the local map. This parameter is used in some approaches about the local map management.");
@@ -295,8 +297,8 @@ class RTABMAP_EXP Parameters
// Local loop closure detection
RTABMAP_PARAM(RGBD, LocalLoopDetectionTime, bool, false, "Detection over all locations in STM.");
RTABMAP_PARAM(RGBD, LocalLoopDetectionSpace, bool, false, "Detection over locations (in Working Memory or STM) near in space.");
RTABMAP_PARAM(RGBD, LocalLoopDetectionNeighbors, int, 20, "Maximum nearest neighbor.");
RTABMAP_PARAM(RGBD, LocalLoopDetectionMaxDiffID, int, 50, "Maximum ID difference between the current/last loop closure location and the local loop closure hypotheses. Set 0 to ignore.")
RTABMAP_PARAM(RGBD, LocalLoopDetectionMaxDiffID, int, 50, "Maximum ID difference between the current/last loop closure location and the local loop closure hypotheses. Set 0 to ignore.");
RTABMAP_PARAM(RGBD, LocalLoopDetectionPathFilteringRadius, float, 1.0, "Path filtering radius.");
// Graph optimization
RTABMAP_PARAM(RGBD, OptimizeStrategy, int, 0, "Graph optimization strategy: 0=TORO and 1=g2o.");
@@ -345,7 +347,8 @@ class RTABMAP_EXP Parameters
// Loop closure constraint
RTABMAP_PARAM(LccIcp, Type, int, 0, "0=No ICP, 1=ICP 3D, 2=ICP 2D");
RTABMAP_PARAM(LccIcp, MaxDistance, float, 0.2, "Maximum ICP correction distance accepted (m).");
RTABMAP_PARAM(LccIcp, MaxTranslation, float, 0.2, "Maximum ICP translation correction accepted (m).");
RTABMAP_PARAM(LccIcp, MaxRotation, float, 0.78, "Maximum ICP rotation correction accepted (rad).");
RTABMAP_PARAM(LccBow, MinInliers, int, 20, "Minimum visual word correspondences to compute geometry transform.");
RTABMAP_PARAM(LccBow, InlierDistance, float, 0.02, "Maximum distance for visual word correspondences.");

9
corelib/include/rtabmap/core/Rtabmap.h
View File

@@ -138,7 +138,8 @@ public:
int getPathCurrentGoalId() const;
const Transform & getPathTransformToGoal() const {return _pathTransformToGoal;}
std::map<int, Transform> getWMPosesInRadius(int fromId, int maxNearestNeighbors, float radius, int maxDiffID, int & nearestId) const;
std::map<int, Transform> getForwardWMPoses(int fromId, int maxNearestNeighbors, float radius, int maxDiffID) const;
std::list<std::map<int, Transform> > getPaths(std::map<int, Transform> poses) const;
void adjustLikelihood(std::map<int, float> & likelihood) const;
std::pair<int, float> selectHypothesis(const std::map<int, float> & posterior,
const std::map<int, float> & likelihood) const;
@@ -174,13 +175,12 @@ private:
float _rgbdAngularUpdate;
float _newMapOdomChangeDistance;
int _globalLoopClosureIcpType;
float _globalLoopClosureIcpMaxDistance;
bool _poseScanMatching;
bool _localLoopClosureDetectionTime;
bool _localLoopClosureDetectionSpace;
float _localRadius;
float _localDetectMaxNeighbors;
int _localDetectMaxDiffID;
float _localPathFilteringRadius;
std::string _databasePath;
bool _optimizeFromGraphEnd;
bool _reextractLoopClosureFeatures;
@@ -190,7 +190,8 @@ private:
int _reextractMaxWords;
bool _startNewMapOnLoopClosure;
float _goalReachedRadius; // meters
bool _planWithNearNodesLinked;
bool _planVirtualLinks;
int _planVirtualLinksMaxDiffID;
bool _goalsSavedInUserData;
std::pair<int, float> _loopClosureHypothesis;

8
corelib/include/rtabmap/core/Statistics.h
View File

@@ -53,6 +53,7 @@ namespace rtabmap {
class RTABMAP_EXP Statistics
{
RTABMAP_STATS(Loop, RejectedHypothesis,);
RTABMAP_STATS(Loop, Accepted_hypothesis_id,);
RTABMAP_STATS(Loop, Highest_hypothesis_id,);
RTABMAP_STATS(Loop, Highest_hypothesis_value,);
RTABMAP_STATS(Loop, Vp_hypothesis,);
@@ -64,9 +65,10 @@ class RTABMAP_EXP Statistics
RTABMAP_STATS(LocalLoop, Odom_corrected,);
RTABMAP_STATS(LocalLoop, Time_closures,);
RTABMAP_STATS(LocalLoop, Space_closure_id,);
RTABMAP_STATS(LocalLoop, Space_nearest_id,);
RTABMAP_STATS(LocalLoop, Space_neighbors,);
RTABMAP_STATS(LocalLoop, Space_last_closure_id,);
RTABMAP_STATS(LocalLoop, Space_paths,);
RTABMAP_STATS(LocalLoop, Space_closures_added,);
RTABMAP_STATS(LocalLoop, Space_closures_added_icp_only,);
RTABMAP_STATS(Memory, Working_memory_size,);
RTABMAP_STATS(Memory, Short_time_memory_size,);