normalize variance in loop closure

corelib/src/Memory.cpp

@@ -2235,6 +2235,12 @@ Transform Memory::computeTransform(
 					info->rejectedMsg = msg;
 				}
 			}
+			else if(info && !transform.isIdentity())
+			{
+				//normalize variance
+				info->varianceLin *= transform.getNorm();
+				info->varianceAng *= transform.getAngle();
+			}
 		}
 	}
 	return transform;
@@ -2282,6 +2288,13 @@ Transform Memory::computeIcpTransform(
 		// compute transform fromId -> toId
 		std::vector<int> inliersV;
 		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
 	{

guilib/src/DatabaseViewer.cpp

@@ -4194,6 +4194,9 @@ void DatabaseViewer::refineConstraint(int from, int to, bool silent, bool update
 
 	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);
 
 		bool updated = false;
@@ -4290,8 +4293,11 @@ bool DatabaseViewer::addConstraint(int from, int to, bool silent, bool updateGra
 			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);
 		}
 		else if(!silent)

guilib/src/MainWindow.cpp

@@ -4735,6 +4735,12 @@ void MainWindow::postProcessing()
 									UINFO("Added new loop closure between %d and %d.", from, to);
 									addedLinks.insert(from);
 									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)));
 									++loopClosuresAdded;
 									_initProgressDialog->appendText(tr("Detected loop closure %1->%2! (%3/%4)").arg(from).arg(to).arg(i+1).arg(clusters.size()));