i/rtabmap
1
0
Fork 0
mirror of https://github.com/introlab/rtabmap.git synced 2026-10-11 12:29:50 +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
corelib/src/Rtabmap.cpp
+5 -6
View File
@@ -851,7 +851,7 @@ void Rtabmap::setInitialPose(const Transform & initialPose)
if(!_memory->isIncremental()) if(!_memory->isIncremental())
{ {
_lastLocalizationPose = initialPose; _lastLocalizationPose = initialPose;
_localizationCovariance = 0; _localizationCovariance = cv::Mat();
_lastLocalizationNodeId = 0; _lastLocalizationNodeId = 0;
_odomCachePoses.clear(); _odomCachePoses.clear();
_odomCacheConstraints.clear(); _odomCacheConstraints.clear();
@@ -1692,11 +1692,11 @@ bool Rtabmap::process(
odomCovariance.type() == CV_64FC1 && odomCovariance.type() == CV_64FC1 &&
odomCovariance.at<double>(0,0) < 1) 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 #ifdef RTABMAP_MRPT
// Transform odometry covariance (which in base frame) into global frame // Transform odometry covariance (which in base frame) into global frame
@@ -1713,9 +1713,8 @@ bool Rtabmap::process(
// If variance is different for each axis, // If variance is different for each axis,
// build rtabmap with MRPT to use approach above. // build rtabmap with MRPT to use approach above.
_localizationCovariance += odomCovariance; _localizationCovariance += odomCovariance;
#endif
} }
#endif
} }
_lastLocalizationPose = newPose; // keep in cache the latest corrected pose _lastLocalizationPose = newPose; // keep in cache the latest corrected pose
if(!_memory->isIncremental() && signature->getWeight() >= 0) if(!_memory->isIncremental() && signature->getWeight() >= 0)