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

Localization covariance empty till localization

Browse Source
This commit is contained in:
matlabbe
2023-06-05 20:08:21 -07:00
parent 1e145550df
commit 1cc7c2818f
1 changed files with 5 additions and 6 deletions
Download Patch File Download Diff File Expand all files Collapse all files

11
corelib/src/Rtabmap.cpp
View File

@@ -851,7 +851,7 @@ void Rtabmap::setInitialPose(const Transform & initialPose)
if(!_memory->isIncremental())
{
_lastLocalizationPose = initialPose;
_localizationCovariance = 0;
_localizationCovariance = cv::Mat();
_lastLocalizationNodeId = 0;
_odomCachePoses.clear();
_odomCacheConstraints.clear();
@@ -1692,11 +1692,11 @@ bool Rtabmap::process(
odomCovariance.type() == CV_64FC1 &&
odomCovariance.at<double>(0,0) < 1)
{
if(_localizationCovariance.empty() || _lastLocalizationPose.isNull())
if( _memory->isIncremental() && _localizationCovariance.empty())
{
_localizationCovariance = odomCovariance.clone();
_localizationCovariance = cv::Mat::zeros(6,6,CV_64FC1);
}
else
if(_localizationCovariance.total() == 36)
{
#ifdef RTABMAP_MRPT
// Transform odometry covariance (which in base frame) into global frame
@@ -1713,9 +1713,8 @@ bool Rtabmap::process(
// If variance is different for each axis,
// build rtabmap with MRPT to use approach above.
_localizationCovariance += odomCovariance;
#endif
}
#endif
}
_lastLocalizationPose = newPose; // keep in cache the latest corrected pose
if(!_memory->isIncremental() && signature->getWeight() >= 0)