Optimizer: added optimizeIncremental() method to solve optimization error problems when re-opening the database (when graph should be re-optimized from raw odometry poses, not latest optimized poses, #172)

corelib/include/rtabmap/core/Optimizer.h

@@ -87,6 +87,14 @@ public:
 
 	virtual void parseParameters(const ParametersMap & parameters);
 
+	std::map<int, Transform> optimizeIncremental(
+			int rootId,
+			const std::map<int, Transform> & poses,
+			const std::multimap<int, Link> & constraints,
+			std::list<std::map<int, Transform> > * intermediateGraphes = 0,
+			double * finalError = 0,
+			int * iterationsDone = 0);
+
 	// inherited classes should implement one of these methods
 	virtual std::map<int, Transform> optimize(
 			int rootId,

corelib/src/Optimizer.cpp

@@ -263,6 +263,66 @@ void Optimizer::parseParameters(const ParametersMap & parameters)
 	Parameters::parse(parameters, Parameters::kOptimizerPriorsIgnored(), priorsIgnored_);
 }
 
+std::map<int, Transform> Optimizer::optimizeIncremental(
+		int rootId,
+		const std::map<int, Transform> & poses,
+		const std::multimap<int, Link> & constraints,
+		std::list<std::map<int, Transform> > * intermediateGraphes,
+		double * finalError,
+		int * iterationsDone)
+{
+	std::map<int, Transform> incGraph;
+	std::multimap<int, Link> incGraphLinks;
+	incGraph.insert(*poses.begin());
+	int i=0;
+	std::multimap<int, Link> constraintsCpy = constraints;
+	UDEBUG("Incremental optimization... poses=%d comstraints=%d", (int)poses.size(), (int)constraints.size());
+	for(std::map<int, Transform>::const_iterator iter=poses.begin(); iter!=poses.end(); ++iter)
+	{
+		bool hasLoopClosure = false;
+		for(std::multimap<int, Link>::iterator jter=constraintsCpy.lower_bound(iter->first); jter!=constraintsCpy.end() && jter->first==iter->first; ++jter)
+		{
+			if(jter->second.type() == Link::kNeighbor || jter->second.type() == Link::kNeighborMerged)
+			{
+				incGraph.insert(std::make_pair(jter->second.to(), incGraph.at(iter->first) * jter->second.transform()));
+				incGraphLinks.insert(*jter);
+			}
+			else
+			{
+				if(!uContains(incGraph, jter->second.to()) && jter->second.to() > iter->first)
+				{
+					// node not yet in graph, switch link direction
+					constraintsCpy.insert(std::make_pair(jter->second.to(), jter->second.inverse()));
+				}
+				else
+				{
+					UASSERT(uContains(incGraph, jter->second.to()));
+					incGraphLinks.insert(*jter);
+					hasLoopClosure = true;
+				}
+			}
+		}
+		if(hasLoopClosure)
+		{
+			incGraph = this->optimize(incGraph.begin()->first, incGraph, incGraphLinks);
+			if(incGraph.empty())
+			{
+				UWARN("Failed incremental optimization...");
+				break;
+			}
+		}
+		UDEBUG("Iteration %d/%d %s", ++i, (int)poses.size(), hasLoopClosure?"*":"");
+	}
+	if(!incGraph.empty() && incGraph.size() == poses.size())
+	{
+		UASSERT(incGraphLinks.size() == constraints.size());
+		return this->optimize(rootId, incGraph, incGraphLinks, intermediateGraphes, finalError, iterationsDone);
+	}
+
+	UDEBUG("Failed incremental optimization");
+	return std::map<int, Transform>();
+}
+
 std::map<int, Transform> Optimizer::optimize(
 		int rootId,
 		const std::map<int, Transform> & poses,

corelib/src/Rtabmap.cpp

@@ -3254,6 +3254,29 @@ std::map<int, Transform> Rtabmap::optimizeGraph(
 	else
 	{
 		optimizedPoses = _graphOptimizer->optimize(fromId, poses, edgeConstraints, 0, error, iterationsDone);
+
+		if(!poses.empty() && optimizedPoses.empty() && guessPoses.empty())
+		{
+			UERROR("Optimization has failed, trying incremental optimization instead, this may take a while (poses=%d, links=%d)...", (int)poses.size(), (int)edgeConstraints.size());
+			optimizedPoses = _graphOptimizer->optimizeIncremental(fromId, poses, edgeConstraints, 0, error, iterationsDone);
+
+			if(optimizedPoses.empty())
+			{
+				if(!_graphOptimizer->isCovarianceIgnored() || _graphOptimizer->type() != Optimizer::kTypeTORO)
+				{
+					UERROR("Incremental optimization also failed. You may try changing parameters to %s=0 and %s=true.",
+							Parameters::kOptimizerStrategy().c_str(), Parameters::kOptimizerVarianceIgnored().c_str());
+				}
+				else
+				{
+					UERROR("Incremental optimization also failed.");
+				}
+			}
+			else
+			{
+				UERROR("Incremental optimization succeeded!");
+			}
+		}
 	}
 	UINFO("Optimization time %f s", timer.ticks());
 

guilib/include/rtabmap/gui/DatabaseViewer.h

@@ -212,6 +212,8 @@ private:
 	bool savedMaximized_;
 	bool firstCall_;
 	QString iniFilePath_;
+
+	bool useLastOptimizedGraphAsGuess_;
 };
 
 }

guilib/src/DatabaseViewer.cpp

@@ -100,7 +100,8 @@ DatabaseViewer::DatabaseViewer(const QString & ini, QWidget * parent) :
 	editDepthDialog_(new QDialog(this)),
 	savedMaximized_(false),
 	firstCall_(true),
-	iniFilePath_(ini)
+	iniFilePath_(ini),
+	useLastOptimizedGraphAsGuess_(false)
 {
 	pathDatabase_ = QDir::homePath()+"/Documents/RTAB-Map"; //use home directory by default
 
@@ -970,6 +971,8 @@ bool DatabaseViewer::closeDatabase()
 		stereoViewer_->update();
 
 		ui_->toolBox_statistics->clear();
+
+		useLastOptimizedGraphAsGuess_ = false;
 	}
 
 	ui_->actionClose_database->setEnabled(dbDriver_ != 0);
@@ -4815,6 +4818,12 @@ void DatabaseViewer::updateGraphView()
 			return;
 		}
 
+		std::map<int, Transform> optimizedGraphGuess;
+		if(graphes_.size() && useLastOptimizedGraphAsGuess_)
+		{
+			optimizedGraphGuess = graphes_.back();
+		}
+
 		graphes_.clear();
 		graphLinks_.clear();
 
@@ -5014,6 +5023,22 @@ void DatabaseViewer::updateGraphView()
 				posesOut,
 				linksOut,
 				ui_->spinBox_optimizationDepth->value());
+		if(optimizedGraphGuess.size() == posesOut.size())
+		{
+			bool identical=true;
+			for(std::map<int, Transform>::iterator iter=posesOut.begin(); iter!=posesOut.end(); ++iter)
+			{
+				if(!uContains(optimizedGraphGuess, iter->first))
+				{
+					identical = false;
+					break;
+				}
+			}
+			if(identical)
+			{
+				posesOut = optimizedGraphGuess;
+			}
+		}
 		UINFO("Connected graph of %d poses and %d links", (int)posesOut.size(), (int)linksOut.size());
 		QTime time;
 		time.start();
@@ -5024,16 +5049,32 @@ void DatabaseViewer::updateGraphView()
 		ui_->label_poses->setNum((int)finalPoses.size());
 		if(posesOut.size() && finalPoses.empty())
 		{
+			UWARN("Optimization failed, trying incremental optimization instead... this may take a while (poses=%d, links=%d).", (int)posesOut.size(), (int)linksOut.size());
+			finalPoses = optimizer->optimizeIncremental(fromId, posesOut, linksOut, &graphes_);
+
+			if(finalPoses.empty())
+			{
+				UWARN("Incremental optimization also failed.");
 				if(!optimizer->isCovarianceIgnored() || optimizer->type() != Optimizer::kTypeTORO)
 				{
 					QMessageBox::warning(this, tr("Graph optimization error!"), tr("Graph optimization has failed. See the terminal for potential errors. "
-						"Give a try with %1=0 and %2=true.").arg(Parameters::kOptimizerStrategy().c_str()).arg(Parameters::kOptimizerVarianceIgnored().c_str()));
+							"Give it a try with %1=0 and %2=true.").arg(Parameters::kOptimizerStrategy().c_str()).arg(Parameters::kOptimizerVarianceIgnored().c_str()));
 				}
 				else
 				{
 					QMessageBox::warning(this, tr("Graph optimization error!"), tr("Graph optimization has failed. See the terminal for potential errors."));
 				}
 			}
+			else
+			{
+				UWARN("Incremental optimization succeeded!");
+				QMessageBox::information(this, tr("Incremental optimization succeeded!"), tr("Graph optimization has failed but "
+						"incremental optimization succeeded. Next optimizations will use the current "
+						"best optimized poses as first guess instead of odometry poses."));
+				useLastOptimizedGraphAsGuess_ = true;
+
+			}
+		}
 		delete optimizer;
 
 		if(uContains(groundTruthPoses_, fromId) && uContains(posesOut, fromId))