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

Pnp multicam refactoring (#902)

Browse Source 
* gui: fixed wrongly showing landmark rejected when it was not (because a loop closure was rejected at the same time)

* Added Vis/PnPMaxVariance and RGBD/InvertedReg parameters. Implemented inlier distribution computation for multicam.

* On loc/small displacement: don't remove from odom cache if loop is rejected (maybe first loc)

* Loc: don't prune odom cache on small movement if delayed loc is enabled

* loc/small movement: cleanup bidirectional links

* Cov/PnP: fixed objPt transform to estimate depth

Co-authored-by: mathieu86 <[email protected]>
This commit is contained in:
matlabbe
2022-09-24 12:29:42 -07:00
committed by GitHub
co-authored by mathieu86
parent 95a76cb696
commit fa31affea0
14 changed files with 685 additions and 495 deletions
Download Patch File Download Diff File Expand all files Collapse all files
corelib/src/Rtabmap.cpp
+47 -21
View File
@@ -1434,32 +1434,45 @@ bool Rtabmap::process(
//============================================================
// Minimum displacement required to add to Memory
//============================================================
const std::multimap<int, Link> & links = signature->getLinks();
if(links.size() && links.begin()->second.type() == Link::kNeighbor)
Transform t;
if(_memory->isIncremental())
{
const Signature * s = _memory->getSignature(links.begin()->second.to());
UASSERT(s!=0);
// don't filter if the new node is not intermediate but previous one is
if(signature->getWeight() < 0 || s->getWeight() >= 0)
const std::multimap<int, Link> & links = signature->getLinks();
if(links.size() && links.begin()->second.type() == Link::kNeighbor)
{
float x,y,z, roll,pitch,yaw;
links.begin()->second.transform().getTranslationAndEulerAngles(x,y,z, roll,pitch,yaw);
bool isMoving = fabs(x) > _rgbdLinearUpdate ||
fabs(y) > _rgbdLinearUpdate ||
fabs(z) > _rgbdLinearUpdate ||
(_rgbdAngularUpdate>0.0f && (
fabs(roll) > _rgbdAngularUpdate ||
fabs(pitch) > _rgbdAngularUpdate ||
fabs(yaw) > _rgbdAngularUpdate));
if(!isMoving)
const Signature * s = _memory->getSignature(links.begin()->second.to());
UASSERT(s!=0);
// don't filter if the new node is not intermediate but previous one is
if(signature->getWeight() < 0 || s->getWeight() >= 0)
{
// This will disable global loop closure detection, only retrieval will be done.
// The location will also be deleted at the end.
smallDisplacement = true;
UDEBUG("smallDisplacement: %f %f %f %f %f %f", x,y,z, roll,pitch,yaw);
t = links.begin()->second.transform();
}
}
}
else if(!_odomCachePoses.empty())
{
t = _odomCachePoses.rbegin()->second.inverse() * signature->getPose();
}
if(!t.isNull())
{
float x,y,z, roll,pitch,yaw;
t.getTranslationAndEulerAngles(x,y,z, roll,pitch,yaw);
bool isMoving = fabs(x) > _rgbdLinearUpdate ||
fabs(y) > _rgbdLinearUpdate ||
fabs(z) > _rgbdLinearUpdate ||
(_rgbdAngularUpdate>0.0f && (
fabs(roll) > _rgbdAngularUpdate ||
fabs(pitch) > _rgbdAngularUpdate ||
fabs(yaw) > _rgbdAngularUpdate));
if(!isMoving)
{
// This will disable global loop closure detection, only retrieval will be done.
// The location will also be deleted at the end.
smallDisplacement = true;
UDEBUG("smallDisplacement: %f %f %f %f %f %f", x,y,z, roll,pitch,yaw);
}
}
}
if(odomVelocity.size() == 6)
{
@@ -2951,6 +2964,7 @@ bool Rtabmap::process(
cv::Mat localizationCovariance;
Transform previousMapCorrection;
bool rejectedLandmark = false;
bool delayedLocalization = false;
UDEBUG("RGB-D SLAM mode: %d", _rgbdSlamMode?1:0);
UDEBUG("Incremental: %d", _memory->isIncremental());
UDEBUG("Loop hyp: %d", _loopClosureHypothesis.first);
@@ -3441,6 +3455,7 @@ bool Rtabmap::process(
else //delayed localization (wait for more than 1 link)
{
UWARN("Localization was good, but waiting for another one to be more accurate (%s>0)", Parameters::kRGBDMaxOdomCacheSize().c_str());
delayedLocalization = true;
rejectLocalization = true;
}
}
@@ -3954,10 +3969,21 @@ bool Rtabmap::process(
(smallDisplacement || tooFastMovement) &&
_loopClosureHypothesis.first == 0 &&
lastProximitySpaceClosureId == 0 &&
!delayedLocalization &&
(rejectedLandmark || landmarksDetected.empty()))
{
_odomCachePoses.erase(signatureRemoved);
_odomCacheConstraints.erase(signatureRemoved);
for(std::multimap<int, Link>::iterator iter=_odomCacheConstraints.begin(); iter!=_odomCacheConstraints.end();)
{
if(iter->second.from() == signatureRemoved || iter->second.to() == signatureRemoved)
{
_odomCacheConstraints.erase(iter++);
}
else
{
++iter;
}
}
}
// Pass this point signature should not be used, since it could have been transferred...