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

RtabmapThread: use largest covariance instead of summation

Browse Source
This commit is contained in:
matlabbe
2017-07-13 11:36:55 -04:00
parent 3c758ab2f6
commit b4cfa0e844
2 changed files with 10 additions and 13 deletions
Download Patch File Download Diff File Expand all files Collapse all files

21
corelib/src/RtabmapThread.cpp
View File

@@ -570,8 +570,7 @@ void RtabmapThread::addData(const OdometryEvent & odomEvent)
}
if(!lastPose_.isIdentity() &&
(odomEvent.pose().isIdentity() ||
odomEvent.rotVariance()>=9999 ||
odomEvent.transVariance()>=9999))
odomEvent.info().covariance.at<double>(0,0)>=9999))
{
if(odomEvent.pose().isIdentity())
{
@@ -579,21 +578,21 @@ void RtabmapThread::addData(const OdometryEvent & odomEvent)
}
else
{
UWARN("Odometry is reset (high variance (%f/%f >=9999 detected). Increment map id!", odomEvent.transVariance(), odomEvent.rotVariance());
UWARN("Odometry is reset (high variance (%f >=9999 detected). Increment map id!", odomEvent.info().covariance.at<double>(0,0));
}
pushNewState(kStateTriggeringMap);
covariance_ = cv::Mat();
}
double maxRotVar = odomEvent.rotVariance();
double maxTransVar = odomEvent.transVariance();
if(maxRotVar != 1.0f && maxTransVar != 1.0f && !covariance_.empty())
if(uIsFinite(odomEvent.info().covariance.at<double>(0,0)) &&
odomEvent.info().covariance.at<double>(0,0) != 1.0 &&
odomEvent.info().covariance.at<double>(0,0)>0.0)
{
covariance_ += odomEvent.covariance();
}
else
{
covariance_ = odomEvent.covariance();
// Use largest covariance error (to be independent of the odometry frame rate)
if(covariance_.empty() || odomEvent.info().covariance.at<double>(0,0) > covariance_.at<double>(0,0))
{
covariance_ = odomEvent.info().covariance;
}
}
if(ignoreFrame && !_createIntermediateNodes)