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/Memory.cpp
+30 -2
View File
@@ -101,6 +101,7 @@ Memory::Memory(const ParametersMap & parameters) :
_laserScanGroundNormalsUp(Parameters::defaultIcpPointToPlaneGroundNormalsUp()),
_reextractLoopClosureFeatures(Parameters::defaultRGBDLoopClosureReextractFeatures()),
_localBundleOnLoopClosure(Parameters::defaultRGBDLocalBundleOnLoopClosure()),
_invertedReg(Parameters::defaultRGBDInvertedReg()),
_rehearsalMaxDistance(Parameters::defaultRGBDLinearUpdate()),
_rehearsalMaxAngle(Parameters::defaultRGBDAngularUpdate()),
_rehearsalWeightIgnoredWhileMoving(Parameters::defaultMemRehearsalWeightIgnoredWhileMoving()),
@@ -578,6 +579,15 @@ void Memory::parseParameters(const ParametersMap & parameters)
Parameters::parse(params, Parameters::kIcpPointToPlaneGroundNormalsUp(), _laserScanGroundNormalsUp);
Parameters::parse(params, Parameters::kRGBDLoopClosureReextractFeatures(), _reextractLoopClosureFeatures);
Parameters::parse(params, Parameters::kRGBDLocalBundleOnLoopClosure(), _localBundleOnLoopClosure);
Parameters::parse(params, Parameters::kRGBDInvertedReg(), _invertedReg);
if(_invertedReg && _localBundleOnLoopClosure)
{
UWARN("%s and %s cannot be used at the same time, disabling %s...",
Parameters::kRGBDLocalBundleOnLoopClosure().c_str(),
Parameters::kRGBDInvertedReg().c_str(),
Parameters::kRGBDLocalBundleOnLoopClosure().c_str());
_localBundleOnLoopClosure = false;
}
Parameters::parse(params, Parameters::kRGBDLinearUpdate(), _rehearsalMaxDistance);
Parameters::parse(params, Parameters::kRGBDAngularUpdate(), _rehearsalMaxAngle);
Parameters::parse(params, Parameters::kMemRehearsalWeightIgnoredWhileMoving(), _rehearsalWeightIgnoredWhileMoving);
@@ -2855,8 +2865,21 @@ Transform Memory::computeTransform(
(fromS.getWords().size() && toS.getWords().size()) ||
(!guess.isNull() && !_registrationPipeline->isImageRequired()))
{
Signature tmpFrom = fromS;
Signature tmpTo = toS;
Signature tmpFrom, tmpTo;
if(_invertedReg)
{
tmpFrom = toS;
tmpTo = fromS;
if(!guess.isNull())
{
guess = guess.inverse();
}
}
else
{
tmpFrom = fromS;
tmpTo = toS;
}
if(_reextractLoopClosureFeatures && (_registrationPipeline->isImageRequired() || guess.isNull()))
{
@@ -2891,6 +2914,7 @@ Transform Memory::computeTransform(
_registrationPipeline->isImageRequired() &&
!_registrationPipeline->isScanRequired() &&
!_registrationPipeline->isUserDataRequired() &&
!_invertedReg &&
!tmpTo.getWordsDescriptors().empty() &&
!tmpTo.getWords().empty() &&
!tmpFrom.getWordsDescriptors().empty() &&
@@ -3115,6 +3139,10 @@ Transform Memory::computeTransform(
{
transform = _registrationPipeline->computeTransformationMod(tmpFrom, tmpTo, guess, info);
}
if(_invertedReg && !transform.isNull())
{
transform = transform.inverse();
}
}
return transform;
}