i/rtabmap
1
0
Fork 0
mirror of https://github.com/introlab/rtabmap.git synced 2026-09-02 01:20:25 +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

2
corelib/include/rtabmap/core/OdometryEvent.h
View File

@@ -86,8 +86,6 @@ public:
return std::vector<float>(); return std::vector<float>();
} }
const OdometryInfo & info() const {return _info;} const OdometryInfo & info() const {return _info;}
double rotVariance() const {return uMax3(_info.covariance.at<double>(3,3), _info.covariance.at<double>(4,4), _info.covariance.at<double>(5,5));}
double transVariance() const {return uMax3(_info.covariance.at<double>(0,0), _info.covariance.at<double>(1,1), _info.covariance.at<double>(2,2));}
private: private:
SensorData _data; SensorData _data;

21
corelib/src/RtabmapThread.cpp
View File

@@ -570,8 +570,7 @@ void RtabmapThread::addData(const OdometryEvent & odomEvent)
} }
if(!lastPose_.isIdentity() && if(!lastPose_.isIdentity() &&
(odomEvent.pose().isIdentity() || (odomEvent.pose().isIdentity() ||
odomEvent.rotVariance()>=9999 || odomEvent.info().covariance.at<double>(0,0)>=9999))
odomEvent.transVariance()>=9999))
{ {
if(odomEvent.pose().isIdentity()) if(odomEvent.pose().isIdentity())
{ {
@@ -579,21 +578,21 @@ void RtabmapThread::addData(const OdometryEvent & odomEvent)
} }
else 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); pushNewState(kStateTriggeringMap);
covariance_ = cv::Mat(); covariance_ = cv::Mat();
} }
double maxRotVar = odomEvent.rotVariance(); if(uIsFinite(odomEvent.info().covariance.at<double>(0,0)) &&
double maxTransVar = odomEvent.transVariance(); odomEvent.info().covariance.at<double>(0,0) != 1.0 &&
if(maxRotVar != 1.0f && maxTransVar != 1.0f && !covariance_.empty()) odomEvent.info().covariance.at<double>(0,0)>0.0)
{ {
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))
else {
{ covariance_ = odomEvent.info().covariance;
covariance_ = odomEvent.covariance(); }
} }
if(ignoreFrame && !_createIntermediateNodes) if(ignoreFrame && !_createIntermediateNodes)