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

normalize variance in loop closure

Browse Source
This commit is contained in:
matlabbe
2017-01-05 17:22:49 -05:00
parent dfb965cfa9
commit 716337b71a
3 changed files with 26 additions and 1 deletions
Download Patch File Download Diff File Expand all files Collapse all files

13
corelib/src/Memory.cpp
View File

@@ -2235,6 +2235,12 @@ Transform Memory::computeTransform(
info->rejectedMsg = msg; info->rejectedMsg = msg;
} }
} }
else if(info && !transform.isIdentity())
{
//normalize variance
info->varianceLin *= transform.getNorm();
info->varianceAng *= transform.getAngle();
}
} }
} }
return transform; return transform;
@@ -2282,6 +2288,13 @@ Transform Memory::computeIcpTransform(
// compute transform fromId -> toId // compute transform fromId -> toId
std::vector<int> inliersV; std::vector<int> inliersV;
t = _registrationIcp->computeTransformation(fromS->sensorData(), toS->sensorData(), guess, info); t = _registrationIcp->computeTransformation(fromS->sensorData(), toS->sensorData(), guess, info);
if(!t.isNull() && !t.isIdentity() && info)
{
// normalize variance
info->varianceLin *= t.getNorm();
info->varianceAng *= t.getAngle();
}
} }
else else
{ {

8
guilib/src/DatabaseViewer.cpp
View File

@@ -4194,6 +4194,9 @@ void DatabaseViewer::refineConstraint(int from, int to, bool silent, bool update
if(!transform.isNull()) if(!transform.isNull())
{ {
// normalize variance
info.varianceLin *= transform.getNorm();
info.varianceAng *= transform.getAngle();
Link newLink(currentLink.from(), currentLink.to(), currentLink.type(), transform, info.varianceAng, info.varianceLin); Link newLink(currentLink.from(), currentLink.to(), currentLink.type(), transform, info.varianceAng, info.varianceLin);
bool updated = false; bool updated = false;
@@ -4290,8 +4293,11 @@ bool DatabaseViewer::addConstraint(int from, int to, bool silent, bool updateGra
updateWordsMatching(); updateWordsMatching();
} }
if(!t.isNull()) if(!t.isNull() && !t.isIdentity())
{ {
// normalize variance
info.varianceLin *= t.getNorm();
info.varianceAng *= t.getAngle();
newLink = Link(from, to, Link::kUserClosure, t, info.varianceAng, info.varianceLin); newLink = Link(from, to, Link::kUserClosure, t, info.varianceAng, info.varianceLin);
} }
else if(!silent) else if(!silent)

6
guilib/src/MainWindow.cpp
View File

@@ -4735,6 +4735,12 @@ void MainWindow::postProcessing()
UINFO("Added new loop closure between %d and %d.", from, to); UINFO("Added new loop closure between %d and %d.", from, to);
addedLinks.insert(from); addedLinks.insert(from);
addedLinks.insert(to); addedLinks.insert(to);
if(!transform.isIdentity())
{
// normalize variance
info.varianceLin *= transform.getNorm();
info.varianceAng *= transform.getAngle();
}
_currentLinksMap.insert(std::make_pair(from, Link(from, to, Link::kUserClosure, transform, info.varianceAng, info.varianceLin))); _currentLinksMap.insert(std::make_pair(from, Link(from, to, Link::kUserClosure, transform, info.varianceAng, info.varianceLin)));
++loopClosuresAdded; ++loopClosuresAdded;
_initProgressDialog->appendText(tr("Detected loop closure %1->%2! (%3/%4)").arg(from).arg(to).arg(i+1).arg(clusters.size())); _initProgressDialog->appendText(tr("Detected loop closure %1->%2! (%3/%4)").arg(from).arg(to).arg(i+1).arg(clusters.size()));