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

Link: Covariances are added on merge when output is NeighborMerged (reduced graph)

Browse Source
This commit is contained in:
matlabbe
2018-11-27 09:23:16 -05:00
parent ca27dbd2fe
commit 0167687c6b
1 changed files with 9 additions and 0 deletions
Download Patch File Download Diff File Expand all files Collapse all files

9
corelib/src/Link.cpp
View File

@@ -112,6 +112,15 @@ Link Link::merge(const Link & link, Type outputType) const
UASSERT((link.transform().isNull() && transform_.isNull()) || (!link.transform().isNull() && !transform_.isNull()));
UASSERT(infMatrix_.cols == 6 && infMatrix_.rows == 6 && infMatrix_.type() == CV_64FC1);
UASSERT(link.infMatrix().cols == 6 && link.infMatrix().rows == 6 && link.infMatrix().type() == CV_64FC1);
if(outputType == kNeighborMerged)
{
return Link(
from_,
link.to(),
outputType,
transform_.isNull()?Transform():transform_ * link.transform(),
transform_.isNull()?cv::Mat::eye(6,6,CV_64FC1):(infMatrix_.inv() + link.infMatrix().inv()).inv());
}
return Link(
from_,
link.to(),