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

odom variance verification limited to yaw if 3dof registration/optimization is done

Browse Source
This commit is contained in:
matlabbe
2018-10-02 18:02:55 -04:00
parent ae08adb8bd
commit 6b5990aa04
1 changed files with 11 additions and 2 deletions
Download Patch File Download Diff File Expand all files Collapse all files

13
corelib/src/Memory.cpp
View File

@@ -824,14 +824,23 @@ void Memory::addSignatureToStm(Signature * signature, const cv::Mat & covariance
!_signatures.at(*_stMem.rbegin())->getPose().isNull()) !_signatures.at(*_stMem.rbegin())->getPose().isNull())
{ {
UASSERT(covariance.cols == 6 && covariance.rows == 6 && covariance.type() == CV_64FC1); UASSERT(covariance.cols == 6 && covariance.rows == 6 && covariance.type() == CV_64FC1);
double maxAngVar = uMax3(covariance.at<double>(3,3), covariance.at<double>(4,4), covariance.at<double>(5,5)); double maxAngVar = 0.0;
if(_registrationPipeline->force3DoF())
{
maxAngVar = covariance.at<double>(5,5);
}
else
{
maxAngVar = uMax3(covariance.at<double>(3,3), covariance.at<double>(4,4), covariance.at<double>(5,5));
}
if(maxAngVar != 1.0 && maxAngVar > 0.1) if(maxAngVar != 1.0 && maxAngVar > 0.1)
{ {
static bool warned = false; static bool warned = false;
if(!warned) if(!warned)
{ {
UWARN("Very large angular variance (%f) detected! Please fix odometry " UWARN("Very large angular variance (%f) detected! Please fix odometry "
"twist covariance, otherwise poor graph optimizations are " "covariance, otherwise poor graph optimizations are "
"expected and wrong loop closure detections creating a lot " "expected and wrong loop closure detections creating a lot "
"of errors in the map could be accepted. This message is only " "of errors in the map could be accepted. This message is only "
"printed once.", maxAngVar); "printed once.", maxAngVar);