From 10c23e60b7d89e654e78fd0ad35e24527b82a81e Mon Sep 17 00:00:00 2001
From: matlabbe <matlabbe@gmail.com>
Date: Wed, 24 May 2017 15:03:27 -0400
Subject: [PATCH] OptimizerG2O: removed fixed frame if a posePrior link is
 detected

---
 corelib/src/OptimizerG2O.cpp | 10 ++++++++++
 1 file changed, 10 insertions(+)

diff --git a/corelib/src/OptimizerG2O.cpp b/corelib/src/OptimizerG2O.cpp
index f195e789..9146c12f 100644
--- a/corelib/src/OptimizerG2O.cpp
+++ b/corelib/src/OptimizerG2O.cpp
@@ -215,6 +215,16 @@ std::map<int, Transform> OptimizerG2O::optimize(
 			optimizer.setAlgorithm(new g2o::OptimizationAlgorithmLevenberg(blockSolver));
 		}
 
+		// detect if there is a global pose prior set, if so remove rootId
+		for(std::multimap<int, Link>::const_iterator iter=edgeConstraints.begin(); iter!=edgeConstraints.end(); ++iter)
+		{
+			if(iter->second.from() == iter->second.to())
+			{
+				rootId = 0;
+				break;
+			}
+		}
+
 		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)