Graph optimization: fixed stop error criterion for g2o and gtsam, previous optimized poses are now used as guess for the optimized graph

corelib/src/toro3d/treeoptimizer2.cpp

@@ -67,7 +67,8 @@ struct ParameterPropagator{
   }
 };
 
-TreeOptimizer2::TreeOptimizer2(){
+TreeOptimizer2::TreeOptimizer2():
+	iteration(1){
   sortedEdges=0;
 }
 
@@ -280,7 +281,8 @@ void TreeOptimizer2::iterate(TreePoseGraph2::EdgeSet* eset){
   if (eset){
     sortedEdges=eset;
   }
-  computePreconditioner();
+  if (iteration==1)
+	  computePreconditioner();
   propagateErrors();
   sortedEdges=temp;
 }