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

Rtabmap: re-optimize at each frame when gravity constraints are added

Browse Source
This commit is contained in:
matlabbe
2019-05-31 19:31:11 -04:00
parent 4c8af6d6e9
commit ec943198f0
4 changed files with 26 additions and 10 deletions
Download Patch File Download Diff File Expand all files Collapse all files

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

@@ -240,12 +240,12 @@ class RTABMAP_EXP Parameters
#ifndef RTABMAP_NONFREE #ifndef RTABMAP_NONFREE
#ifdef RTABMAP_OPENCV3 #ifdef RTABMAP_OPENCV3
// OpenCV 3 without xFeatures2D module doesn't have BRIEF // OpenCV 3 without xFeatures2D module doesn't have BRIEF
RTABMAP_PARAM(Kp, DetectorStrategy, int, 8, "0=SURF 1=SIFT 2=ORB 3=FAST/FREAK 4=FAST/BRIEF 5=GFTT/FREAK 6=GFTT/BRIEF 7=BRISK 8=GFTT/ORB 9=KAZE."); RTABMAP_PARAM(Kp, DetectorStrategy, int, 8, "0=SURF 1=SIFT 2=ORB 3=FAST/FREAK 4=FAST/BRIEF 5=GFTT/FREAK 6=GFTT/BRIEF 7=BRISK 8=GFTT/ORB 9=KAZE 10=ORB-OCTREE.");
#else #else
RTABMAP_PARAM(Kp, DetectorStrategy, int, 6, "0=SURF 1=SIFT 2=ORB 3=FAST/FREAK 4=FAST/BRIEF 5=GFTT/FREAK 6=GFTT/BRIEF 7=BRISK 8=GFTT/ORB 9=KAZE."); RTABMAP_PARAM(Kp, DetectorStrategy, int, 6, "0=SURF 1=SIFT 2=ORB 3=FAST/FREAK 4=FAST/BRIEF 5=GFTT/FREAK 6=GFTT/BRIEF 7=BRISK 8=GFTT/ORB 9=KAZE 10=ORB-OCTREE.");
#endif #endif
#else #else
RTABMAP_PARAM(Kp, DetectorStrategy, int, 6, "0=SURF 1=SIFT 2=ORB 3=FAST/FREAK 4=FAST/BRIEF 5=GFTT/FREAK 6=GFTT/BRIEF 7=BRISK 8=GFTT/ORB 9=KAZE."); RTABMAP_PARAM(Kp, DetectorStrategy, int, 6, "0=SURF 1=SIFT 2=ORB 3=FAST/FREAK 4=FAST/BRIEF 5=GFTT/FREAK 6=GFTT/BRIEF 7=BRISK 8=GFTT/ORB 9=KAZE 10=ORB-OCTREE.");
#endif #endif
RTABMAP_PARAM(Kp, TfIdfLikelihoodUsed, bool, true, "Use of the td-idf strategy to compute the likelihood."); RTABMAP_PARAM(Kp, TfIdfLikelihoodUsed, bool, true, "Use of the td-idf strategy to compute the likelihood.");
RTABMAP_PARAM(Kp, Parallelized, bool, true, "If the dictionary update and signature creation were parallelized."); RTABMAP_PARAM(Kp, Parallelized, bool, true, "If the dictionary update and signature creation were parallelized.");
@@ -567,12 +567,12 @@ class RTABMAP_EXP Parameters
#ifndef RTABMAP_NONFREE #ifndef RTABMAP_NONFREE
#ifdef RTABMAP_OPENCV3 #ifdef RTABMAP_OPENCV3
// OpenCV 3 without xFeatures2D module doesn't have BRIEF // OpenCV 3 without xFeatures2D module doesn't have BRIEF
RTABMAP_PARAM(Vis, FeatureType, int, 8, "0=SURF 1=SIFT 2=ORB 3=FAST/FREAK 4=FAST/BRIEF 5=GFTT/FREAK 6=GFTT/BRIEF 7=BRISK 8=GFTT/ORB 9=KAZE."); RTABMAP_PARAM(Vis, FeatureType, int, 8, "0=SURF 1=SIFT 2=ORB 3=FAST/FREAK 4=FAST/BRIEF 5=GFTT/FREAK 6=GFTT/BRIEF 7=BRISK 8=GFTT/ORB 9=KAZE 10=ORB-OCTREE.");
#else #else
RTABMAP_PARAM(Vis, FeatureType, int, 6, "0=SURF 1=SIFT 2=ORB 3=FAST/FREAK 4=FAST/BRIEF 5=GFTT/FREAK 6=GFTT/BRIEF 7=BRISK 8=GFTT/ORB 9=KAZE."); RTABMAP_PARAM(Vis, FeatureType, int, 6, "0=SURF 1=SIFT 2=ORB 3=FAST/FREAK 4=FAST/BRIEF 5=GFTT/FREAK 6=GFTT/BRIEF 7=BRISK 8=GFTT/ORB 9=KAZE 10=ORB-OCTREE.");
#endif #endif
#else #else
RTABMAP_PARAM(Vis, FeatureType, int, 6, "0=SURF 1=SIFT 2=ORB 3=FAST/FREAK 4=FAST/BRIEF 5=GFTT/FREAK 6=GFTT/BRIEF 7=BRISK 8=GFTT/ORB 9=KAZE."); RTABMAP_PARAM(Vis, FeatureType, int, 6, "0=SURF 1=SIFT 2=ORB 3=FAST/FREAK 4=FAST/BRIEF 5=GFTT/FREAK 6=GFTT/BRIEF 7=BRISK 8=GFTT/ORB 9=KAZE 10=ORB-OCTREE.");
#endif #endif
RTABMAP_PARAM(Vis, MaxFeatures, int, 1000, "0 no limits."); RTABMAP_PARAM(Vis, MaxFeatures, int, 1000, "0 no limits.");
RTABMAP_PARAM(Vis, MaxDepth, float, 0, "Max depth of the features (0 means no limit)."); RTABMAP_PARAM(Vis, MaxDepth, float, 0, "Max depth of the features (0 means no limit).");

2
corelib/include/rtabmap/core/Signature.h
View File

@@ -82,7 +82,7 @@ public:
void addLinks(const std::map<int, Link> & links); void addLinks(const std::map<int, Link> & links);
void addLink(const Link & link); void addLink(const Link & link);
bool hasLink(int idTo) const; bool hasLink(int idTo, Link::Type type = Link::kUndef) const;
void changeLinkIds(int idFrom, int idTo); void changeLinkIds(int idFrom, int idTo);

7
corelib/src/Rtabmap.cpp
View File

@@ -2533,7 +2533,8 @@ bool Rtabmap::process(
(_loopClosureHypothesis.first>0 || (_loopClosureHypothesis.first>0 ||
lastProximitySpaceClosureId>0 || // can be different map of the current one lastProximitySpaceClosureId>0 || // can be different map of the current one
statistics_.reducedIds().size() || statistics_.reducedIds().size() ||
(signature->hasLink(signature->id()) && !_graphOptimizer->priorsIgnored()) || // prior edge (signature->hasLink(signature->id(), Link::kPosePrior) && !_graphOptimizer->priorsIgnored()) || // prior edge
(signature->hasLink(signature->id(), Link::kGravity) && _graphOptimizer->gravitySigma()>0.0f) || // gravity edge
proximityDetectionsInTimeFound>0 || proximityDetectionsInTimeFound>0 ||
landmarkDetected!=0 || landmarkDetected!=0 ||
((_memory->isIncremental() || graph::filterLinks(signature->getLinks(), Link::kSelfRefLink).size()) && // In localization mode, the new node should be linked ((_memory->isIncremental() || graph::filterLinks(signature->getLinks(), Link::kSelfRefLink).size()) && // In localization mode, the new node should be linked
@@ -2720,6 +2721,10 @@ bool Rtabmap::process(
u = signature->getPose() * transform; u = signature->getPose() * transform;
} }
else
{
UWARN("Gravity link not found for %d and/or %d, localization won't be corrected with gravity.", loopId, signature->id());
}
} }
Transform up = u * oldPose.inverse(); Transform up = u * oldPose.inverse();
for(std::map<int, Transform>::iterator iter=_optimizedPoses.begin(); iter!=_optimizedPoses.end(); ++iter) for(std::map<int, Transform>::iterator iter=_optimizedPoses.begin(); iter!=_optimizedPoses.end(); ++iter)

15
corelib/src/Signature.cpp
View File

@@ -126,9 +126,20 @@ void Signature::addLink(const Link & link)
_linksModified = true; _linksModified = true;
} }
bool Signature::hasLink(int idTo) const bool Signature::hasLink(int idTo, Link::Type type) const
{ {
return _links.find(idTo) != _links.end(); if(type == Link::kUndef)
{
return _links.find(idTo) != _links.end();
}
for(std::multimap<int, Link>::const_iterator iter=_links.find(idTo); iter!=_links.end() && iter->first == idTo; ++iter)
{
if(type == iter->second.type())
{
return true;
}
}
return false;
} }
void Signature::changeLinkIds(int idFrom, int idTo) void Signature::changeLinkIds(int idFrom, int idTo)