Merge branch 'dronecrew-g2o_prior' into devel

.gitignore

@@ -1,6 +1,8 @@
 /lib
 .DS_Store
 .settings/language.settings.xml
+.idea/
+cmake-build-debug/
 app/android/.classpath
 app/android/.project
 app/android/AndroidManifest.xml

CMakeLists.txt

@@ -807,3 +807,5 @@ MESSAGE(SEND_ERROR "No graph optimizer found! You should have at least one of th
        GTSAM (https://collab.cc.gatech.edu/borg/gtsam)
        set -DWITH_TORO=ON")
 ENDIF(NOT GTSAM_FOUND AND NOT G2O_FOUND AND NOT WITH_TORO)
+
+# vim: set et ft=cmake fenc=utf-8 ff=unix sts=0 sw=2 ts=2 :

corelib/include/rtabmap/core/Link.h

@@ -46,7 +46,8 @@ public:
 		kUserClosure,
 		kVirtualClosure,
 		kNeighborMerged,
-		kUndef};
+		kPosePrior,
+		kUndef = 99};
 	Link();
 	Link(int from,
 			int to,

corelib/include/rtabmap/core/SensorData.h

@@ -221,6 +221,10 @@ public:
 	void setGroundTruth(const Transform & pose) {groundTruth_ = pose;}
 	const Transform & groundTruth() const {return groundTruth_;}
 
+	void setGlobalPose(const Transform & pose, const cv::Mat & covariance) {globalPose_ = pose; globalPoseCovariance_ = covariance;}
+	const Transform & globalPose() const {return globalPose_;}
+	const cv::Mat & globalPoseCovariance() const {return globalPoseCovariance_;}
+
 	long getMemoryUsed() const; // Return memory usage in Bytes
 
 private:
@@ -258,6 +262,9 @@ private:
 	cv::Mat _descriptors;
 
 	Transform groundTruth_;
+
+	Transform globalPose_;
+	cv::Mat globalPoseCovariance_; // 6x6 double
 };
 
 }

corelib/src/DBDriver.cpp

@@ -1127,7 +1127,7 @@ void DBDriver::generateGraph(
 									 weightNeighbor,
 									 colorG);
 						 }
-						 else
+						 else if(iter->first != id)
 						 {
 							 //child
 							 fprintf(fout, "   \"%d\\n%d\" -> \"%d\\n%d\" [label=\"C\", fontcolor=%s, fontsize=8];\n",

corelib/src/Graph.cpp

@@ -1455,6 +1455,8 @@ std::list<std::pair<int, Transform> > computePath(
 			}
 		}
 		for(std::map<int, Link>::const_iterator iter = links.begin(); iter!=links.end(); ++iter)
+		{
+			if(iter->second.from() != iter->second.to())
 			{
 				Transform nextPose = currentNode->pose()*iter->second.transform();
 				float cost = 0.0f;
@@ -1517,6 +1519,7 @@ std::list<std::pair<int, Transform> > computePath(
 				}
 			}
 		}
+	}
 
 	// Debugging stuff
 	if(ULogger::level() == ULogger::kDebug)

corelib/src/Memory.cpp

@@ -805,6 +805,7 @@ void Memory::moveSignatureToWMFromSTM(int id, int * reducedTo)
 			if(!merge)
 			{
 				merge = iter->second.to() < s->id() && // should be a parent->child link
+						iter->second.to() != iter->second.from() &&
 						iter->second.type() != Link::kNeighbor &&
 						iter->second.type() != Link::kNeighborMerged &&
 						iter->second.userDataCompressed().empty() &&
@@ -958,6 +959,7 @@ std::map<int, Link> Memory::getLoopClosureLinks(
 		{
 			if(iter->second.type() != Link::kNeighbor &&
 			   iter->second.type() != Link::kNeighborMerged &&
+			   iter->second.type() != Link::kPosePrior &&
 			   iter->second.type() != Link::kUndef)
 			{
 				loopClosures.insert(*iter);
@@ -1842,6 +1844,8 @@ void Memory::moveToTrash(Signature * s, bool keepLinkedToGraph, std::list<int> *
 								"STM is not implemented!", s->id()).c_str());
 			const std::map<int, Link> & links = s->getLinks();
 			for(std::map<int, Link>::const_iterator iter=links.begin(); iter!=links.end(); ++iter)
+			{
+				if(iter->second.from() != iter->second.to())
 				{
 					Signature * sTo = this->_getSignature(iter->first);
 					// neighbor to s
@@ -1864,6 +1868,7 @@ void Memory::moveToTrash(Signature * s, bool keepLinkedToGraph, std::list<int> *
 					}
 
 					sTo->removeLink(s->id());
+				}
 
 			}
 			s->removeLinks(); // remove all links
@@ -2089,6 +2094,7 @@ void Memory::removeLink(int oldId, int newId)
 			{
 				if(iter->second.type() != Link::kNeighbor &&
 				   iter->second.type() != Link::kNeighborMerged &&
+				   iter->second.type() != Link::kPosePrior &&
 				   iter->first < newS->id())
 				{
 					noChildrenAnymore = false;
@@ -2694,7 +2700,7 @@ void Memory::dumpMemoryTree(const char * fileNameTree) const
 					{
 						childIds.insert(*iter);
 					}
-					else
+					else if(iter->second.from() != iter->second.to())
 					{
 						loopIds.insert(*iter);
 					}
@@ -2724,8 +2730,7 @@ void Memory::dumpMemoryTree(const char * fileNameTree) const
 void Memory::rehearsal(Signature * signature, Statistics * stats)
 {
 	UTimer timer;
-	if(signature->getLinks().size() != 1 ||
+	if(signature->isBadSignature())
-	   signature->isBadSignature())
 	{
 		return;
 	}
@@ -2790,7 +2795,8 @@ bool Memory::rehearsalMerge(int oldId, int newId)
 		std::map<int, Link>::const_iterator iter = oldS->getLinks().find(newS->id());
 		if(iter != oldS->getLinks().end() &&
 		   iter->second.type() != Link::kNeighbor &&
-		   iter->second.type() != Link::kNeighborMerged)
+		   iter->second.type() != Link::kNeighborMerged &&
+		   iter->second.from() != iter->second.to())
 		{
 			// do nothing, already merged
 			UWARN("already merged, old=%d, new=%d", oldId, newId);
@@ -2804,7 +2810,7 @@ bool Memory::rehearsalMerge(int oldId, int newId)
 
 		bool fullMerge;
 		bool intermediateMerge = false;
-		if(!newS->getLinks().begin()->second.transform().isNull())
+		if(!newS->getLinks().empty() && !newS->getLinks().begin()->second.transform().isNull())
 		{
 			// we are in metric SLAM mode:
 			// 1) Normal merge if not moving AND has direct link
@@ -2843,6 +2849,8 @@ bool Memory::rehearsalMerge(int oldId, int newId)
 				// redirect neighbor links
 				const std::map<int, Link> & links = oldS->getLinks();
 				for(std::map<int, Link>::const_iterator iter = links.begin(); iter!=links.end(); ++iter)
+				{
+					if(iter->second.from() != iter->second.to())
 					{
 						Link link = iter->second;
 						Link mergedLink = newToOldLink.merge(link, link.type());
@@ -2862,6 +2870,7 @@ bool Memory::rehearsalMerge(int oldId, int newId)
 							UERROR("Didn't find neighbor %d of %d in RAM...", link.to(), oldS->id());
 						}
 					}
+				}
 				newS->setLabel(oldS->getLabel());
 				oldS->setLabel("");
 				oldS->removeLinks(); // remove all links
@@ -3847,6 +3856,12 @@ Signature * Memory::createSignature(const SensorData & data, const Transform & p
 	}
 	s->sensorData().setOccupancyGrid(ground, obstacles, cellSize, viewPoint);
 
+	// prior
+	if(!isIntermediateNode && !data.globalPose().isNull() && data.globalPoseCovariance().cols==6 && data.globalPoseCovariance().rows==6 && data.globalPoseCovariance().cols==CV_64FC1)
+	{
+		s->addLink(Link(s->id(), s->id(), Link::kPosePrior, data.globalPose(), data.globalPoseCovariance().inv()));
+	}
+
 	return s;
 }
 

corelib/src/OptimizerG2O.cpp

@@ -47,6 +47,8 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 #include "g2o/core/optimization_algorithm_levenberg.h"
 #include "g2o/core/linear_solver.h"
 #include "g2o/types/sba/types_sba.h"
+#include "g2o/types/slam2d/types_slam2d.h"
+#include "g2o/types/slam3d/types_slam3d.h"
 #include "g2o/core/robust_kernel_impl.h"
 #ifdef G2O_HAVE_CSPARSE
 #include "g2o/solvers/csparse/linear_solver_csparse.h"
@@ -56,10 +58,10 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 #include "g2o/solvers/cholmod/linear_solver_cholmod.h"
 #endif
 #include "g2o/solvers/eigen/linear_solver_eigen.h"
-#include "g2o/types/slam3d/vertex_se3.h"
+
-#include "g2o/types/slam3d/edge_se3.h"
+enum {
-#include "g2o/types/slam2d/vertex_se2.h"
+    PARAM_OFFSET=0,
-#include "g2o/types/slam2d/edge_se2.h"
+};
 
 typedef g2o::BlockSolver< g2o::BlockSolverTraits<-1, -1> > SlamBlockSolver;
 typedef g2o::LinearSolverEigen<SlamBlockSolver::PoseMatrixType> SlamLinearEigenSolver;
@@ -165,6 +167,9 @@ std::map<int, Transform> OptimizerG2O::optimize(
 
 		g2o::SparseOptimizer optimizer;
 		optimizer.setVerbose(ULogger::level()==ULogger::kDebug);
+		g2o::ParameterSE3Offset* odomOffset = new g2o::ParameterSE3Offset();
+		odomOffset->setId(PARAM_OFFSET);
+		optimizer.addParameter(odomOffset);
 
 		SlamBlockSolver * blockSolver = 0;
 
@@ -211,6 +216,7 @@ std::map<int, Transform> OptimizerG2O::optimize(
 		}
 
 		UDEBUG("fill poses to g2o...");
+		std::map<int, std::pair<Transform, cv::Mat> > geoPoses; // pose / information matrix
 		for(std::map<int, Transform>::const_iterator iter = poses.begin(); iter!=poses.end(); ++iter)
 		{
 			UASSERT(!iter->second.isNull());
@@ -255,6 +261,53 @@ std::map<int, Transform> OptimizerG2O::optimize(
 
 			g2o::HyperGraph::Edge * edge = 0;
 
+			if(id1 == id2)
+			{
+				if(isSlam2d())
+				{
+					g2o::EdgeSE2Prior * priorEdge = new g2o::EdgeSE2Prior();
+					g2o::VertexSE2* v1 = (g2o::VertexSE2*)optimizer.vertex(id1);
+					priorEdge->setVertex(0, v1);
+					priorEdge->setMeasurement(g2o::SE2(iter->second.transform().x(), iter->second.transform().y(), iter->second.transform().theta()));
+					priorEdge->setParameterId(0, PARAM_OFFSET);
+					Eigen::Matrix<double, 3, 3> information = Eigen::Matrix<double, 3, 3>::Identity();
+					if(!isCovarianceIgnored())
+					{
+						information(0,0) = iter->second.infMatrix().at<double>(0,0); // x-x
+						information(0,1) = iter->second.infMatrix().at<double>(0,1); // x-y
+						information(0,2) = iter->second.infMatrix().at<double>(0,5); // x-theta
+						information(1,0) = iter->second.infMatrix().at<double>(1,0); // y-x
+						information(1,1) = iter->second.infMatrix().at<double>(1,1); // y-y
+						information(1,2) = iter->second.infMatrix().at<double>(1,5); // y-theta
+						information(2,0) = iter->second.infMatrix().at<double>(5,0); // theta-x
+						information(2,1) = iter->second.infMatrix().at<double>(5,1); // theta-y
+						information(2,2) = iter->second.infMatrix().at<double>(5,5); // theta-theta
+					}
+					priorEdge->setInformation(information);
+					edge = priorEdge;
+				}
+				else
+				{
+					g2o::EdgeSE3Prior * priorEdge = new g2o::EdgeSE3Prior();
+					g2o::VertexSE3* v1 = (g2o::VertexSE3*)optimizer.vertex(id1);
+					priorEdge->setVertex(0, v1);
+					Eigen::Affine3d a = iter->second.transform().toEigen3d();
+					Eigen::Isometry3d pose;
+					pose = a.rotation();
+					pose.translation() = a.translation();
+					priorEdge->setMeasurement(pose);
+					priorEdge->setParameterId(0, PARAM_OFFSET);
+					Eigen::Matrix<double, 6, 6> information = Eigen::Matrix<double, 6, 6>::Identity();
+					if(!isCovarianceIgnored())
+					{
+						memcpy(information.data(), iter->second.infMatrix().data, iter->second.infMatrix().total()*sizeof(double));
+					}
+					priorEdge->setInformation(information);
+					edge = priorEdge;
+				}
+			}
+			else
+			{
 #ifdef RTABMAP_VERTIGO
 				VertexSwitchLinear * v = 0;
 				if(this->isRobust() &&
@@ -378,12 +431,15 @@ std::map<int, Transform> OptimizerG2O::optimize(
 						edge = e;
 					}
 				}
+			}
 
 			if (!optimizer.addEdge(edge))
 			{
 				delete edge;
 				UERROR("Map: Failed adding constraint between %d and %d, skipping", id1, id2);
 			}
+
+
 		}
 
 		UDEBUG("Initial optimization...");

corelib/src/OptimizerGTSAM.cpp

@@ -134,6 +134,11 @@ std::map<int, Transform> OptimizerGTSAM::optimize(
 		{
 			int id1 = iter->second.from();
 			int id2 = iter->second.to();
+			if(id1 == id2)
+			{
+				// not supporting pose prior
+				continue;
+			}
 
 			UASSERT(!iter->second.transform().isNull());
 

corelib/src/OptimizerTORO.cpp

@@ -126,6 +126,8 @@ std::map<int, Transform> OptimizerTORO::optimize(
 
 				int id1 = iter->second.from();
 				int id2 = iter->second.to();
+				if(id1 != id2)
+				{
 					AISNavigation::TreePoseGraph2::Vertex* v1=pg2.vertex(id1);
 					AISNavigation::TreePoseGraph2::Vertex* v2=pg2.vertex(id2);
 					UASSERT(v1 != 0);
@@ -136,6 +138,8 @@ std::map<int, Transform> OptimizerTORO::optimize(
 						UERROR("Map: Edge already exits between nodes %d and %d, skipping", id1, id2);
 					}
 				}
+				//else // not supporting pose prior
+			}
 		}
 		else
 		{
@@ -154,6 +158,8 @@ std::map<int, Transform> OptimizerTORO::optimize(
 
 				int id1 = iter->second.from();
 				int id2 = iter->second.to();
+				if(id1 != id2)
+				{
 					AISNavigation::TreePoseGraph3::Vertex* v1=pg3.vertex(id1);
 					AISNavigation::TreePoseGraph3::Vertex* v2=pg3.vertex(id2);
 					UASSERT(v1 != 0);
@@ -164,6 +170,8 @@ std::map<int, Transform> OptimizerTORO::optimize(
 						UERROR("Map: Edge already exits between nodes %d and %d, skipping", id1, id2);
 					}
 				}
+				//else // not supporting pose prior
+			}
 		}
 		UDEBUG("buildMST... root=%d", rootId);
 		UASSERT(uContains(poses, rootId));

corelib/src/Rtabmap.cpp

@@ -1033,7 +1033,7 @@ bool Rtabmap::process(
 			// Minimum displacement required to add to Memory
 			//============================================================
 			const std::map<int, Link> & links = signature->getLinks();
-			if(links.size() == 1)
+			if(links.size() && links.begin()->second.type() == Link::kNeighbor)
 			{
 				// don't do this if there are intermediate nodes
 				const Signature * s = _memory->getSignature(links.begin()->second.to());
@@ -1061,7 +1061,8 @@ bool Rtabmap::process(
 		// Update optimizedPoses with the newly added node
 		Transform newPose;
 		if(_neighborLinkRefining &&
-			signature->getLinks().size() == 1 &&
+			signature->getLinks().size() &&
+			signature->getLinks().begin()->second.type() == Link::kNeighbor &&
 		   _memory->isIncremental() && // ignore pose matching in localization mode
 		   rehearsedId == 0) // don't do it if rehearsal happened
 		{
@@ -1166,7 +1167,7 @@ bool Rtabmap::process(
 		// Update Poses and Constraints
 		_optimizedPoses.insert(std::make_pair(signature->id(), newPose));
 		_lastLocalizationPose = newPose; // keep in cache the latest corrected pose
-		if(signature->getLinks().size() == 1 &&
+		if(signature->getLinks().size() &&
 		   signature->getLinks().begin()->second.type() == Link::kNeighbor)
 		{
 			// link should be old to new
@@ -1297,7 +1298,7 @@ bool Rtabmap::process(
 	//============================================================
 	// Bayes filter update
 	//============================================================
-	int previousId = signature->getLinks().size() == 1?signature->getLinks().begin()->first:0;
+	int previousId = signature->getLinks().size() && signature->getLinks().begin()->first!=signature->id()?signature->getLinks().begin()->first:0;
 	// Not a bad signature, not an intermediate node, not a small displacement unless the previous signature didn't have a loop closure
 	if(!signature->isBadSignature() && signature->getWeight()>=0 && (!smallDisplacement || _memory->getLoopClosureLinks(previousId, false).size() == 0))
 	{
@@ -1383,7 +1384,7 @@ bool Rtabmap::process(
 			{
 				float loopThr = _loopThr;
 				if((_startNewMapOnLoopClosure || !_memory->isIncremental()) &&
-					signature->getLinks().size() == 0 &&     // alone in the current map
+					graph::filterLinks(signature->getLinks(), Link::kPosePrior).size() == 0 && // alone in the current map
 					_memory->getWorkingMem().size()>1 && // should have an old map (beside virtual signature)
 					(int)_memory->getWorkingMem().size()<=_memory->getMaxStMemSize() &&
 					_rgbdSlamMode)
@@ -2154,14 +2155,16 @@ bool Rtabmap::process(
 		(_loopClosureHypothesis.first>0 ||
 	     lastProximitySpaceClosureId>0 || // can be different map of the current one
 	     statistics_.reducedIds().size() ||
+		 signature->hasLink(signature->id()) || // prior edge
 	     proximityDetectionsInTimeFound>0 ||
-		 ((_memory->isIncremental() || signature->getLinks().size()) && // In localization mode, the new node should be linked
+		 ((_memory->isIncremental() || graph::filterLinks(signature->getLinks(), Link::kPosePrior).size()) && // In localization mode, the new node should be linked
 		          signaturesRetrieved.size())))  		// can be different map of the current one
 	{
 		UASSERT(uContains(_optimizedPoses, signature->id()));
 
 		//used in localization mode: filter virtual links
 		std::map<int, Link> localizationLinks = graph::filterLinks(signature->getLinks(), Link::kVirtualClosure);
+		localizationLinks = graph::filterLinks(localizationLinks, Link::kPosePrior);
 
 		// Note that in localization mode, we don't re-optimize the graph
 		// if:
@@ -2239,7 +2242,7 @@ bool Rtabmap::process(
 				for(std::multimap<int, Link>::iterator iter=constraints.begin(); iter!=constraints.end(); ++iter)
 				{
 					// ignore links with high variance
-					if(iter->second.transVariance() <= 1.0)
+					if(iter->second.transVariance() <= 1.0 && iter->second.from() != iter->second.to())
 					{
 						Transform t1 = uValue(poses, iter->second.from(), Transform());
 						Transform t2 = uValue(poses, iter->second.to(), Transform());
@@ -2437,7 +2440,7 @@ bool Rtabmap::process(
 
 	Signature lastSignatureData(signature->id());
 	Transform lastSignatureLocalizedPose;
-	if(_optimizedPoses.find(signature->id()) != _optimizedPoses.end() && signature->getLinks().size())
+	if(_optimizedPoses.find(signature->id()) != _optimizedPoses.end() && graph::filterLinks(signature->getLinks(), Link::kPosePrior).size())
 	{
 		// only if localized set it
 		lastSignatureLocalizedPose = _optimizedPoses.at(signature->id());
@@ -2466,7 +2469,7 @@ bool Rtabmap::process(
 	{
 		if(_startNewMapOnLoopClosure &&
 			_memory->isIncremental() &&              // only in mapping mode
-			signature->getLinks().size() == 0 &&     // alone in the current map
+			graph::filterLinks(signature->getLinks(), Link::kPosePrior).size() == 0 &&     // alone in the current map
 			_memory->getWorkingMem().size()>=2)       // The working memory should not be empty (beside virtual signature)
 		{
 			UWARN("Ignoring location %d because a global loop closure is required before starting a new map!",
@@ -3121,18 +3124,26 @@ std::map<int, Transform> Rtabmap::optimizeGraph(
 				}
 			}
 		}
+		int ignoredLinks = 0;
 		if(edgeConstraints.size() != linksOut.size())
 		{
 			for(std::multimap<int, Link>::iterator iter=edgeConstraints.begin(); iter!=edgeConstraints.end(); ++iter)
 			{
 				if(graph::findLink(linksOut, iter->second.from(), iter->second.to()) == linksOut.end())
+				{
+					if(iter->second.type() == Link::kPosePrior)
+					{
+						++ignoredLinks;
+					}
+					else
 					{
 						UERROR("Not found link %d->%d in linksOut", iter->second.from(), iter->second.to());
 					}
 				}
 			}
-		UASSERT_MSG(poses.size() == posesOut.size() && edgeConstraints.size() == linksOut.size(),
+		}
-				uFormat("nodes %d->%d, links %d->%d", poses.size(), posesOut.size(), edgeConstraints.size(), linksOut.size()).c_str());
+		UASSERT_MSG(poses.size() == posesOut.size() && edgeConstraints.size()-ignoredLinks == linksOut.size(),
+				uFormat("nodes %d->%d, links %d->%d (ignored=%d)", poses.size(), posesOut.size(), edgeConstraints.size(), linksOut.size(), ignoredLinks).c_str());
 	}
 
 	if(constraints)
@@ -3143,7 +3154,7 @@ std::map<int, Transform> Rtabmap::optimizeGraph(
 	UASSERT(_graphOptimizer!=0);
 	if(_graphOptimizer->iterations() == 0)
 	{
-		// Optimization desactivated! Return not optimized poses.
+		// Optimization disabled! Return not optimized poses.
 		optimizedPoses = poses;
 	}
 	else
@@ -3574,7 +3585,7 @@ int Rtabmap::detectMoreLoopClosures(float clusterRadius, float clusterAngle, int
 								for(std::multimap<int, Link>::iterator iter=links.begin(); iter!=links.end(); ++iter)
 								{
 									// ignore links with high variance
-									if(iter->second.transVariance() <= 1.0)
+									if(iter->second.transVariance() <= 1.0 && iter->second.from() != iter->second.to())
 									{
 										UASSERT(optimizedPoses.find(iter->second.from())!=optimizedPoses.end());
 										UASSERT(optimizedPoses.find(iter->second.to())!=optimizedPoses.end());
@@ -3906,7 +3917,7 @@ bool Rtabmap::computePath(const Transform & targetPose)
 		for(std::map<int, Link>::const_iterator jter=s->getLinks().begin(); jter!=s->getLinks().end(); ++jter)
 		{
 			// only add links for which poses are in "nodes"
-			if(uContains(nodes, jter->second.to()))
+			if(jter->second.from() != jter->second.to() && uContains(nodes, jter->second.to()))
 			{
 				links.insert(std::make_pair(jter->second.from(), jter->second.to()));
 				//links.insert(std::make_pair(jter->second.to(), jter->second.from())); // <-> (commented: already added when iterating in nodes)

corelib/src/Signature.cpp

@@ -120,7 +120,7 @@ void Signature::addLink(const Link & link)
 {
 	UDEBUG("Add link %d to %d (type=%d var=%f,%f)", link.to(), this->id(), (int)link.type(), link.transVariance(), link.rotVariance());
 	UASSERT_MSG(link.from() == this->id(), uFormat("%d->%d for signature %d (type=%d)", link.from(), link.to(), this->id(), link.type()).c_str());
-	UASSERT_MSG(link.to() != this->id(), uFormat("%d->%d for signature %d (type=%d)", link.from(), link.to(), this->id(), link.type()).c_str());
+	UASSERT_MSG((link.to() != this->id()) || link.type()==Link::kPosePrior, uFormat("%d->%d for signature %d (type=%d)", link.from(), link.to(), this->id(), link.type()).c_str());
 	std::pair<std::map<int, Link>::iterator, bool> pair = _links.insert(std::make_pair(link.to(), link));
 	UASSERT_MSG(pair.second, uFormat("Link %d (type=%d) already added to signature %d!", link.to(), link.type(), this->id()).c_str());
 	_linksModified = true;

guilib/src/DatabaseViewer.cpp

@@ -4879,7 +4879,7 @@ bool DatabaseViewer::addConstraint(int from, int to, bool silent)
 			for(std::multimap<int, Link>::iterator iter=links.begin(); iter!=links.end(); ++iter)
 			{
 				// ignore links with high variance
-				if(iter->second.transVariance() <= 1.0)
+				if(iter->second.transVariance() <= 1.0 && iter->second.from() != iter->second.to())
 				{
 					UASSERT(poses.find(iter->second.from())!=poses.end());
 					UASSERT(poses.find(iter->second.to())!=poses.end());

guilib/src/MainWindow.cpp

@@ -5137,7 +5137,7 @@ void MainWindow::postProcessing()
 											for(std::multimap<int, Link>::iterator iter=links.begin(); iter!=links.end(); ++iter)
 											{
 												// ignore links with high variance
-												if(iter->second.transVariance() <= 1.0)
+												if(iter->second.transVariance() <= 1.0 && iter->second.from() != iter->second.to())
 												{
 													UASSERT(poses.find(iter->second.from())!=poses.end());
 													UASSERT(poses.find(iter->second.to())!=poses.end());