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

Added check if covariance is invertible, if not an error is thrown

Browse Source
This commit is contained in:
matlabbe
2017-07-12 16:41:47 -04:00
parent dc77bb4332
commit 78bdd4a087
1 changed files with 8 additions and 0 deletions
Download Patch File Download Diff File Expand all files Collapse all files

8
corelib/src/Memory.cpp
View File

@@ -718,7 +718,15 @@ void Memory::addSignatureToStm(Signature * signature, const cv::Mat & covariance
if(!signature->getPose().isNull() && if(!signature->getPose().isNull() &&
!_signatures.at(*_stMem.rbegin())->getPose().isNull()) !_signatures.at(*_stMem.rbegin())->getPose().isNull())
{ {
UASSERT(covariance.cols == 6 && covariance.rows == 6 && covariance.type() == CV_64FC1);
cv::Mat infMatrix = covariance.inv(); cv::Mat infMatrix = covariance.inv();
if((uIsFinite(covariance.at<double>(0,0) && covariance.at<double>(0,0)>0)) &&
!(uIsFinite(infMatrix.at<double>(0,0) && infMatrix.at<double>(0,0)>0)))
{
UERROR("Failed to invert the covariance matrix! Covariance matrix should be invertible!");
std::cout << "Covariance: " << covariance << std::endl;
infMatrix = cv::Mat::eye(6,6,CV_64FC1);
}
motionEstimate = _signatures.at(*_stMem.rbegin())->getPose().inverse() * signature->getPose(); motionEstimate = _signatures.at(*_stMem.rbegin())->getPose().inverse() * signature->getPose();
_signatures.at(*_stMem.rbegin())->addLink(Link(*_stMem.rbegin(), signature->id(), Link::kNeighbor, motionEstimate, infMatrix)); _signatures.at(*_stMem.rbegin())->addLink(Link(*_stMem.rbegin(), signature->id(), Link::kNeighbor, motionEstimate, infMatrix));
signature->addLink(Link(signature->id(), *_stMem.rbegin(), Link::kNeighbor, motionEstimate.inverse(), infMatrix)); signature->addLink(Link(signature->id(), *_stMem.rbegin(), Link::kNeighbor, motionEstimate.inverse(), infMatrix));