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::setInitialPose() update optimized poses if empty

Browse Source
This commit is contained in:
matlabbe
2019-03-27 22:07:06 -04:00
parent 09ced82c9f
commit 35933cafba
1 changed files with 7 additions and 0 deletions
Download Patch File Download Diff File Expand all files Collapse all files

7
corelib/src/Rtabmap.cpp
View File

@@ -684,6 +684,13 @@ void Rtabmap::setInitialPose(const Transform & initialPose)
_lastLocalizationNodeId = 0; _lastLocalizationNodeId = 0;
_mapCorrection.setIdentity(); _mapCorrection.setIdentity();
_mapCorrectionBackup.setNull(); _mapCorrectionBackup.setNull();
if(_memory->getLastWorkingSignature()->id() &&
_optimizedPoses.empty())
{
cv::Mat covariance;
this->optimizeCurrentMap(_memory->getLastWorkingSignature()->id(), false, _optimizedPoses, covariance, &_constraints);
}
} }
else else
{ {