Merged master to velodyne

corelib/include/rtabmap/core/Graph.h

@@ -82,7 +82,9 @@ public:
 			int rootId,
 			const std::map<int, Transform> & poses,
 			const std::multimap<int, Link> & constraints,
-			std::list<std::map<int, Transform> > * intermediateGraphes = 0);
+			std::list<std::map<int, Transform> > * intermediateGraphes = 0,
+			double * finalError = 0,
+			int * iterationsDone = 0);
 	virtual std::map<int, Transform> optimizeBA(
 			int rootId,
 			const std::map<int, Transform> & poses,
@@ -137,7 +139,9 @@ public:
 			int rootId,
 			const std::map<int, Transform> & poses,
 			const std::multimap<int, Link> & edgeConstraints,
-			std::list<std::map<int, Transform> > * intermediateGraphes = 0);
+			std::list<std::map<int, Transform> > * intermediateGraphes = 0,
+			double * finalError = 0,
+			int * iterationsDone = 0);
 };
 
 class RTABMAP_EXP G2OOptimizer : public Optimizer
@@ -169,7 +173,9 @@ public:
 			int rootId,
 			const std::map<int, Transform> & poses,
 			const std::multimap<int, Link> & edgeConstraints,
-			std::list<std::map<int, Transform> > * intermediateGraphes = 0);
+			std::list<std::map<int, Transform> > * intermediateGraphes = 0,
+			double * finalError = 0,
+			int * iterationsDone = 0);
 };
 
 class RTABMAP_EXP GTSAMOptimizer : public Optimizer
@@ -196,7 +202,9 @@ public:
 			int rootId,
 			const std::map<int, Transform> & poses,
 			const std::multimap<int, Link> & edgeConstraints,
-			std::list<std::map<int, Transform> > * intermediateGraphes = 0);
+			std::list<std::map<int, Transform> > * intermediateGraphes = 0,
+			double * finalError = 0,
+			int * iterationsDone = 0);
 };
 
 class RTABMAP_EXP CVSBAOptimizer : public Optimizer

corelib/include/rtabmap/core/Rtabmap.h

@@ -153,12 +153,17 @@ private:
 	void optimizeCurrentMap(int id,
 			bool lookInDatabase,
 			std::map<int, Transform> & optimizedPoses,
-			std::multimap<int, Link> * constraints = 0) const;
+			std::multimap<int, Link> * constraints = 0,
+			double * error = 0,
+			int * iterationsDone = 0) const;
 	std::map<int, Transform> optimizeGraph(
 			int fromId,
 			const std::set<int> & ids,
+			const std::map<int, Transform> & guessPoses,
 			bool lookInDatabase,
-			std::multimap<int, Link> * constraints = 0) const;
+			std::multimap<int, Link> * constraints = 0,
+			double * error = 0,
+			int * iterationsDone = 0) const;
 	void updateGoalIndex();
 	bool computePath(int targetNode, std::map<int, Transform> nodes, const std::multimap<int, rtabmap::Link> & constraints);
 

corelib/include/rtabmap/core/Statistics.h

@@ -63,6 +63,8 @@ class RTABMAP_EXP Statistics
 	RTABMAP_STATS(Loop, Visual_inliers,);
 	RTABMAP_STATS(Loop, Last_id,);
 	RTABMAP_STATS(Loop, Optimization_max_error, m);
+	RTABMAP_STATS(Loop, Optimization_error, );
+	RTABMAP_STATS(Loop, Optimization_iterations, );
 
 	RTABMAP_STATS(LocalLoop, Time_closures,);
 	RTABMAP_STATS(LocalLoop, Space_last_closure_id,);

corelib/src/Graph.cpp

@@ -201,7 +201,9 @@ std::map<int, Transform> Optimizer::optimize(
 		int rootId,
 		const std::map<int, Transform> & poses,
 		const std::multimap<int, Link> & constraints,
-		std::list<std::map<int, Transform> > * intermediateGraphes)
+		std::list<std::map<int, Transform> > * intermediateGraphes,
+		double * finalError,
+		int * iterationsDone)
 {
 	UERROR("Optimizer %d doesn't implement optimize() method.", (int)this->type());
 	return std::map<int, Transform>();
@@ -291,7 +293,9 @@ std::map<int, Transform> TOROOptimizer::optimize(
 		int rootId,
 		const std::map<int, Transform> & poses,
 		const std::multimap<int, Link> & edgeConstraints,
-		std::list<std::map<int, Transform> > * intermediateGraphes) // contains poses after tree init to last one before the end
+		std::list<std::map<int, Transform> > * intermediateGraphes, // contains poses after tree init to last one before the end
+		double * finalError,
+		int * iterationsDone)
 {
 	std::map<int, Transform> optimizedPoses;
 	UDEBUG("Optimizing graph (pose=%d constraints=%d)...", (int)poses.size(), (int)edgeConstraints.size());
@@ -403,8 +407,8 @@ std::map<int, Transform> TOROOptimizer::optimize(
 		if(isSlam2d())
 		{
 			pg2.buildMST(rootId); // pg.buildSimpleTree();
-			UDEBUG("initializeOnTree()");
-			pg2.initializeOnTree();
+			//UDEBUG("initializeOnTree()");
+			//pg2.initializeOnTree();
 			UDEBUG("initializeTreeParameters()");
 			pg2.initializeTreeParameters();
 			UDEBUG("Building TORO tree... (if a crash happens just after this msg, "
@@ -414,8 +418,8 @@ std::map<int, Transform> TOROOptimizer::optimize(
 		else
 		{
 			pg3.buildMST(rootId); // pg.buildSimpleTree();
-			UDEBUG("initializeOnTree()");
-			pg3.initializeOnTree();
+			//UDEBUG("initializeOnTree()");
+			//pg3.initializeOnTree();
 			UDEBUG("initializeTreeParameters()");
 			pg3.initializeTreeParameters();
 			UDEBUG("Building TORO tree... (if a crash happens just after this msg, "
@@ -423,9 +427,9 @@ std::map<int, Transform> TOROOptimizer::optimize(
 			pg3.initializeOptimization();
 		}
 
+		UINFO("Initial error = %f", pg2.error());
 		UINFO("TORO optimizing begin (iterations=%d)", iterations());
-		double lasterror = 0;
-		double errorDelta = 0;
+		double lastError = 0;
 		int i=0;
 		UTimer timer;
 		for (; i<iterations(); i++)
@@ -482,15 +486,35 @@ std::map<int, Transform> TOROOptimizer::optimize(
 			}
 
 			// early stop condition
-			errorDelta = lasterror - error;
+			double errorDelta = lastError - error;
 			if(i>0 && errorDelta < this->epsilon())
 			{
-				UDEBUG("Stop optimizing, not enough improvement (%f < %f)", errorDelta, this->epsilon());
+				if(errorDelta < 0)
+				{
+					UDEBUG("Negative improvement?! Ignore and continue optimizing... (%f < %f)", errorDelta, this->epsilon());
+				}
+				else
+				{
+					UINFO("Stop optimizing, not enough improvement (%f < %f)", errorDelta, this->epsilon());
 					break;
 				}
-			lasterror = error;
 			}
-		UINFO("TORO optimizing end (%d iterations done, error=%f, time = %f s)", i, errorDelta, timer.ticks());
+			else if(i==0 && error < this->epsilon())
+			{
+				UINFO("Stop optimizing, error is already under epsilon (%f < %f)", error, this->epsilon());
+				break;
+			}
+			lastError = error;
+		}
+		if(finalError)
+		{
+			*finalError = lastError;
+		}
+		if(iterationsDone)
+		{
+			*iterationsDone = i;
+		}
+		UINFO("TORO optimizing end (%d iterations done, error=%f, time = %f s)", i, lastError, timer.ticks());
 
 		if(isSlam2d())
 		{
@@ -713,7 +737,9 @@ std::map<int, Transform> G2OOptimizer::optimize(
 		int rootId,
 		const std::map<int, Transform> & poses,
 		const std::multimap<int, Link> & edgeConstraints,
-		std::list<std::map<int, Transform> > * intermediateGraphes)
+		std::list<std::map<int, Transform> > * intermediateGraphes,
+		double * finalError,
+		int * iterationsDone)
 {
 	std::map<int, Transform> optimizedPoses;
 #ifdef WITH_G2O
@@ -935,9 +961,12 @@ std::map<int, Transform> G2OOptimizer::optimize(
 		UINFO("g2o optimizing begin (max iterations=%d, robust=%d)", iterations(), isRobust()?1:0);
 		int it = 0;
 		UTimer timer;
-		if(intermediateGraphes)
+		double lastError = 0.0;
+		if(intermediateGraphes || this->epsilon() > 0.0)
 		{
 			for(int i=0; i<iterations(); ++i)
+			{
+				if(intermediateGraphes)
 				{
 					if(i > 0)
 					{
@@ -980,13 +1009,33 @@ std::map<int, Transform> G2OOptimizer::optimize(
 						}
 						intermediateGraphes->push_back(tmpPoses);
 					}
+				}
 
 				it += optimizer.optimize(1);
-				if(ULogger::level() == ULogger::kDebug)
-				{
+
+				// early stop condition
 				optimizer.computeActiveErrors();
-					UDEBUG("iteration %d: %d nodes, %d edges, chi2: %f", i, (int)optimizer.vertices().size(), (int)optimizer.edges().size(), optimizer.activeRobustChi2());
+				double chi2 = optimizer.activeRobustChi2();
+				UDEBUG("iteration %d: %d nodes, %d edges, chi2: %f", i, (int)optimizer.vertices().size(), (int)optimizer.edges().size(), chi2);
+				double errorDelta = lastError - chi2;
+				if(i>0 && errorDelta < this->epsilon())
+				{
+					if(errorDelta < 0)
+					{
+						UDEBUG("Negative improvement?! Ignore and continue optimizing... (%f < %f)", errorDelta, this->epsilon());
 					}
+					else
+					{
+						UINFO("Stop optimizing, not enough improvement (%f < %f)", errorDelta, this->epsilon());
+						break;
+					}
+				}
+				else if(i==0 && chi2 < this->epsilon())
+				{
+					UINFO("Stop optimizing, error is already under epsilon (%f < %f)", chi2, this->epsilon());
+					break;
+				}
+				lastError = chi2;
 			}
 		}
 		else
@@ -995,6 +1044,14 @@ std::map<int, Transform> G2OOptimizer::optimize(
 			optimizer.computeActiveErrors();
 			UDEBUG("%d nodes, %d edges, chi2: %f", (int)optimizer.vertices().size(), (int)optimizer.edges().size(), optimizer.activeRobustChi2());
 		}
+		if(finalError)
+		{
+			*finalError = lastError;
+		}
+		if(iterationsDone)
+		{
+			*iterationsDone = it;
+		}
 		UINFO("g2o optimizing end (%d iterations done, error=%f, time = %f s)", it, optimizer.activeRobustChi2(), timer.ticks());
 
 		if(isSlam2d())
@@ -1163,7 +1220,9 @@ std::map<int, Transform> GTSAMOptimizer::optimize(
 		int rootId,
 		const std::map<int, Transform> & poses,
 		const std::multimap<int, Link> & edgeConstraints,
-		std::list<std::map<int, Transform> > * intermediateGraphes)
+		std::list<std::map<int, Transform> > * intermediateGraphes,
+		double * finalError,
+		int * iterationsDone)
 {
 	std::map<int, Transform> optimizedPoses;
 #ifdef WITH_GTSAM
@@ -1303,6 +1362,8 @@ std::map<int, Transform> GTSAMOptimizer::optimize(
 
 		UINFO("GTSAM optimizing begin (max iterations=%d, robust=%d)", iterations(), isRobust()?1:0);
 		UTimer timer;
+		int it = 0;
+		double lastError = 0.0;
 		for(int i=0; i<iterations(); ++i)
 		{
 			if(intermediateGraphes && i > 0)
@@ -1329,18 +1390,45 @@ std::map<int, Transform> GTSAMOptimizer::optimize(
 			try
 			{
 				optimizer.iterate();
+				++it;
 			}
 			catch(gtsam::IndeterminantLinearSystemException & e)
 			{
 				UERROR("GTSAM exception catched: %s", e.what());
 				return optimizedPoses;
 			}
-			UDEBUG("iteration %d error =%f", i+1, optimizer.error());
-			if(optimizer.error() < epsilon())
+
+			// early stop condition
+			double error = optimizer.error();
+			UDEBUG("iteration %d error =%f", i+1, error);
+			double errorDelta = lastError - error;
+			if(i>0 && errorDelta < this->epsilon())
 			{
+				if(errorDelta < 0)
+				{
+					UDEBUG("Negative improvement?! Ignore and continue optimizing... (%f < %f)", errorDelta, this->epsilon());
+				}
+				else
+				{
+					UINFO("Stop optimizing, not enough improvement (%f < %f)", errorDelta, this->epsilon());
 					break;
 				}
 			}
+			else if(i==0 && error < this->epsilon())
+			{
+				UINFO("Stop optimizing, error is already under epsilon (%f < %f)", error, this->epsilon());
+				break;
+			}
+			lastError = error;
+		}
+		if(finalError)
+		{
+			*finalError = lastError;
+		}
+		if(iterationsDone)
+		{
+			*iterationsDone = it;
+		}
 		UINFO("GTSAM optimizing end (%d iterations done, error=%f (initial=%f final=%f), time=%f s)", optimizer.iterations(), optimizer.error(), graph.error(initialEstimate), graph.error(optimizer.values()), timer.ticks());
 
 		for(gtsam::Values::const_iterator iter=optimizer.values().begin(); iter!=optimizer.values().end(); ++iter)

corelib/src/Rtabmap.cpp

@@ -1895,7 +1895,7 @@ bool Rtabmap::process(
 								if(_localPathOdomPosesUsed)
 								{
 									//optimize the path's poses locally
-									path = optimizeGraph(nearestId, uKeysSet(path), false);
+									path = optimizeGraph(nearestId, uKeysSet(path), std::map<int, Transform>(), false);
 									// transform local poses in optimized graph referential
 									UASSERT(uContains(path, nearestId));
 									Transform t = _optimizedPoses.at(nearestId) * path.at(nearestId).inverse();
@@ -2021,6 +2021,8 @@ bool Rtabmap::process(
 	// Optimize map graph
 	//============================================================
 	float maxLinearError = 0.0f;
+	double optimizationError = 0.0;
+	int optimizationIterations = 0;
 	if(_rgbdSlamMode &&
 		(_loopClosureHypothesis.first>0 ||
 	     lastLocalSpaceClosureId>0 || // can be different map of the current one
@@ -2071,7 +2073,7 @@ bool Rtabmap::process(
 			UINFO("Update map correction");
 			std::map<int, Transform> poses = _optimizedPoses;
 			std::multimap<int, Link> constraints;
-			optimizeCurrentMap(signature->id(), false, poses, &constraints);
+			optimizeCurrentMap(signature->id(), false, poses, &constraints, &optimizationError, &optimizationIterations);
 			UASSERT(poses.find(signature->id()) != poses.end());
 
 			// Check added loop closures have broken the graph
@@ -2195,6 +2197,8 @@ bool Rtabmap::process(
 			statistics_.addStatistic(Statistics::kLoopVisual_inliers(), loopClosureVisualInliers);
 			statistics_.addStatistic(Statistics::kLoopLast_id(), _memory->getLastGlobalLoopClosureId());
 			statistics_.addStatistic(Statistics::kLoopOptimization_max_error(), maxLinearError);
+			statistics_.addStatistic(Statistics::kLoopOptimization_error(), optimizationError);
+			statistics_.addStatistic(Statistics::kLoopOptimization_iterations(), optimizationIterations);
 
 			statistics_.addStatistic(Statistics::kLocalLoopTime_closures(), localLoopClosuresInTimeFound);
 			statistics_.addStatistic(Statistics::kLocalLoopSpace_closures_added_visually(), localSpaceClosuresAddedVisually);
@@ -2783,7 +2787,9 @@ void Rtabmap::optimizeCurrentMap(
 		int id,
 		bool lookInDatabase,
 		std::map<int, Transform> & optimizedPoses,
-		std::multimap<int, Link> * constraints) const
+		std::multimap<int, Link> * constraints,
+		double * error,
+		int * iterationsDone) const
 {
 	//Optimize the map
 	UINFO("Optimize map: around location %d", id);
@@ -2797,7 +2803,7 @@ void Rtabmap::optimizeCurrentMap(
 		}
 		UINFO("get %d ids time %f s", (int)ids.size(), timer.ticks());
 
-		std::map<int, Transform> poses = Rtabmap::optimizeGraph(id, uKeysSet(ids), lookInDatabase, constraints);
+		std::map<int, Transform> poses = Rtabmap::optimizeGraph(id, uKeysSet(ids), optimizedPoses, lookInDatabase, constraints, error, iterationsDone);
 		UINFO("optimize time %f s", timer.ticks());
 
 		if(poses.size())
@@ -2820,8 +2826,11 @@ void Rtabmap::optimizeCurrentMap(
 std::map<int, Transform> Rtabmap::optimizeGraph(
 		int fromId,
 		const std::set<int> & ids,
+		const std::map<int, Transform> & guessPoses,
 		bool lookInDatabase,
-		std::multimap<int, Link> * constraints) const
+		std::multimap<int, Link> * constraints,
+		double * error,
+		int * iterationsDone) const
 {
 	UTimer timer;
 	std::map<int, Transform> optimizedPoses;
@@ -2831,6 +2840,19 @@ std::map<int, Transform> Rtabmap::optimizeGraph(
 	_memory->getMetricConstraints(ids, poses, edgeConstraints, lookInDatabase);
 	UINFO("get constraints (ids=%d, %d poses, %d edges) time %f s", (int)ids.size(), (int)poses.size(), (int)edgeConstraints.size(), timer.ticks());
 
+	// Apply guess poses (if some)
+	if(_graphOptimizer->iterations() > 0)
+	{
+		for(std::map<int, Transform>::const_iterator iter=guessPoses.begin(); iter!=guessPoses.end(); ++iter)
+		{
+			std::map<int, Transform>::iterator foundPose = poses.find(iter->first);
+			if(foundPose!=poses.end())
+			{
+				foundPose->second = iter->second;
+			}
+		}
+	}
+
 	// The constraints must be all already connected! Only check in debug
 	if(ULogger::level() == ULogger::kDebug)
 	{
@@ -2879,7 +2901,7 @@ std::map<int, Transform> Rtabmap::optimizeGraph(
 	}
 	else
 	{
-		optimizedPoses = _graphOptimizer->optimize(fromId, poses, edgeConstraints);
+		optimizedPoses = _graphOptimizer->optimize(fromId, poses, edgeConstraints, 0, error, iterationsDone);
 	}
 	UINFO("Optimization time %f s", timer.ticks());
 
@@ -3024,6 +3046,7 @@ void Rtabmap::get3DMap(
 		{
 			if(optimized)
 			{
+				poses = _optimizedPoses; // guess
 				this->optimizeCurrentMap(_memory->getLastWorkingSignature()->id(), global, poses, &constraints);
 			}
 			else
@@ -3099,6 +3122,7 @@ void Rtabmap::getGraph(
 		{
 			if(optimized)
 			{
+				poses = _optimizedPoses; // guess
 				this->optimizeCurrentMap(_memory->getLastWorkingSignature()->id(), global, poses, &constraints);
 			}
 			else

corelib/src/toro3d/treeoptimizer2.cpp

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