Replaced all optimizeIncremental() by optimizeMultiSession() to fix GTSAM errors when merging multiple maps together. DBReader: always publish saved stamp. DBViewer: added actions to set covariance of all neighbor or loop closure links.

corelib/include/rtabmap/core/DBDriver.h

@@ -103,7 +103,7 @@ public:
 	void savePreviewImage(const cv::Mat & image) const;
 	cv::Mat loadPreviewImage() const;
 	void saveOptimizedPoses(const std::map<int, Transform> & optimizedPoses, const Transform & lastlocalizationPose) const;
-	std::map<int, Transform> loadOptimizedPoses(Transform * lastlocalizationPose) const;
+	std::map<int, Transform> loadOptimizedPoses(Transform * lastlocalizationPose = 0) const;
 	void save2DMap(const cv::Mat & map, float xMin, float yMin, float cellSize) const;
 	cv::Mat load2DMap(float & xMin, float & yMin, float & cellSize) const;
 	void saveOptimizedMesh(
@@ -232,7 +232,7 @@ protected:
 	virtual void savePreviewImageQuery(const cv::Mat & image) const = 0;
 	virtual cv::Mat loadPreviewImageQuery() const = 0;
 	virtual void saveOptimizedPosesQuery(const std::map<int, Transform> & optimizedPoses, const Transform & lastlocalizationPose) const = 0;
-	virtual std::map<int, Transform> loadOptimizedPosesQuery(Transform * lastlocalizationPose) const = 0;
+	virtual std::map<int, Transform> loadOptimizedPosesQuery(Transform * lastlocalizationPose = 0) const = 0;
 	virtual void save2DMapQuery(const cv::Mat & map, float xMin, float yMin, float cellSize) const = 0;
 	virtual cv::Mat load2DMapQuery(float & xMin, float & yMin, float & cellSize) const = 0;
 	virtual void saveOptimizedMeshQuery(

corelib/include/rtabmap/core/DBDriverSqlite3.h

@@ -104,7 +104,7 @@ protected:
 	virtual void savePreviewImageQuery(const cv::Mat & image) const;
 	virtual cv::Mat loadPreviewImageQuery() const;
 	virtual void saveOptimizedPosesQuery(const std::map<int, Transform> & optimizedPoses, const Transform & lastlocalizationPose) const;
-	virtual std::map<int, Transform> loadOptimizedPosesQuery(Transform * lastlocalizationPose) const;
+	virtual std::map<int, Transform> loadOptimizedPosesQuery(Transform * lastlocalizationPose = 0) const;
 	virtual void save2DMapQuery(const cv::Mat & map, float xMin, float yMin, float cellSize) const;
 	virtual cv::Mat load2DMapQuery(float & xMin, float & yMin, float & cellSize) const;
 	virtual void saveOptimizedMeshQuery(

corelib/include/rtabmap/core/Graph.h

@@ -135,6 +135,9 @@ std::multimap<int, int>::const_iterator RTABMAP_EXP findLink(
 		int from,
 		int to,
 		bool checkBothWays = true);
+std::list<Link> RTABMAP_EXP findLinks(
+		const std::multimap<int, Link> & links,
+		int from);
 
 std::multimap<int, Link> RTABMAP_EXP filterDuplicateLinks(
 		const std::multimap<int, Link> & links);

corelib/include/rtabmap/core/Optimizer.h

@@ -95,6 +95,14 @@ public:
 			double * finalError = 0,
 			int * iterationsDone = 0);
 
+	std::map<int, Transform> optimizeMultiSession(
+				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);
+
 	std::map<int, Transform> optimize(
 				int rootId,
 				const std::map<int, Transform> & poses,

corelib/src/DBReader.cpp

@@ -400,10 +400,6 @@ SensorData DBReader::getNextData(CameraInfo * info)
 				_previousStamp = stamp;
 				_previousMapID = mapId;
 			}
-			else
-			{
-				stamp = 0;
-			}
 
 			data.uncompressData();
 			if(data.cameraModels().size() > 1 &&

corelib/src/Graph.cpp

@@ -1037,6 +1037,25 @@ std::multimap<int, int>::const_iterator findLink(
 	return links.end();
 }
 
+std::list<Link> findLinks(
+		const std::multimap<int, Link> & links,
+		int from)
+{
+	std::list<Link> output;
+	for(std::multimap<int, Link>::const_iterator iter=links.begin(); iter != links.end(); ++iter)
+	{
+		if(iter->second.from() == from)
+		{
+			output.push_back(iter->second);
+		}
+		else if(iter->second.to() == from)
+		{
+			output.push_back(iter->second.inverse());
+		}
+	}
+	return output;
+}
+
 std::multimap<int, Link> filterDuplicateLinks(
 		const std::multimap<int, Link> & links)
 {

corelib/src/Optimizer.cpp

@@ -276,7 +276,7 @@ std::map<int, Transform> Optimizer::optimizeIncremental(
 	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());
+	UDEBUG("Incremental optimization... poses=%d constraints=%d", (int)poses.size(), (int)constraints.size());
 	for(std::map<int, Transform>::const_iterator iter=poses.begin(); iter!=poses.end(); ++iter)
 	{
 		incGraph.insert(*iter);
@@ -310,7 +310,7 @@ std::map<int, Transform> Optimizer::optimizeIncremental(
 			incGraph = this->optimize(incGraph.begin()->first, incGraph, incGraphLinks);
 			if(incGraph.empty())
 			{
-				UWARN("Failed incremental optimization...");
+				UWARN("Failed incremental optimization... last pose added is %d", iter->first);
 				break;
 			}
 		}
@@ -328,6 +328,70 @@ std::map<int, Transform> Optimizer::optimizeIncremental(
 	return std::map<int, Transform>();
 }
 
+std::map<int, Transform> Optimizer::optimizeMultiSession(
+		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;
+	if(poses.empty())
+	{
+		return incGraph;
+	}
+
+	UDEBUG("Incremental optimization... poses=%d constraints=%d", (int)poses.size(), (int)constraints.size());
+	std::set<int> nextPoses;
+	nextPoses.insert(rootId);
+	UASSERT(uContains(poses, rootId));
+	while(!nextPoses.empty())
+	{
+		std::set<int> currentPoses = nextPoses;
+		nextPoses.clear();
+		for(std::set<int>::iterator iter=currentPoses.begin(); iter!=currentPoses.end(); ++iter)
+		{
+			int fromId = *iter;
+			if(incGraph.empty())
+			{
+				std::map<int, Transform>::const_iterator cter = poses.find(fromId);
+				UASSERT(cter != poses.end());
+				incGraph.insert(*cter);
+			}
+			std::list<Link> links = graph::findLinks(constraints, fromId);
+			for(std::list<Link>::iterator jter = links.begin(); jter!=links.end(); ++jter)
+			{
+				if(!uContains(incGraph, jter->to()))
+				{
+					incGraph.insert(std::make_pair(jter->to(), incGraph.at(jter->from()) * jter->transform()));
+					nextPoses.insert(jter->to());
+				}
+			}
+		}
+	}
+	UASSERT(!incGraph.empty());
+
+	if(intermediateGraphes)
+	{
+		intermediateGraphes->push_back(incGraph);
+	}
+
+	if(incGraph.size() != poses.size())
+	{
+		UWARN("Failed multi-session optimization, output poses (%d) != input poses (%d)", incGraph.size(), poses.size());
+	}
+	else
+	{
+		UASSERT(uContains(poses, rootId) && uContains(incGraph, rootId));
+		incGraph.at(rootId) = poses.at(rootId);
+		return this->optimize(rootId, incGraph, constraints, 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/RegistrationVis.cpp

@@ -1595,7 +1595,7 @@ Transform RegistrationVis::computeTransformationImpl(
 				if((int)allInliers.size() < _minInliers)
 				{
 					msg = uFormat("Not enough inliers after bundle adjustment %d/%d (matches=%d) between %d and %d",
-							(int)allInliers.size(), _minInliers, fromSignature.id(), toSignature.id());
+							(int)allInliers.size(), _minInliers, (int)allInliers.size()+sbaOutliers.size(), fromSignature.id(), toSignature.id());
 					transforms[0].setNull();
 				}
 				else

corelib/src/Rtabmap.cpp

@@ -1073,7 +1073,7 @@ bool Rtabmap::process(
 		UFATAL("Not supposed to be here...last signature is null?!?");
 	}
 
-	ULOGGER_INFO("Processing signature %d w=%d", signature->id(), signature->getWeight());
+	ULOGGER_INFO("Processing signature %d w=%d map=%d", signature->id(), signature->getWeight(), signature->mapId());
 	timeMemoryUpdate = timer.ticks();
 	ULOGGER_INFO("timeMemoryUpdate=%fs", timeMemoryUpdate);
 
@@ -2370,12 +2370,14 @@ bool Rtabmap::process(
 					UINFO("Max optimization linear error = %f m (link %d->%d, var=%f, ratio error/std=%f)", maxLinearError, maxLinearLink->from(), maxLinearLink->to(), maxLinearLink->transVariance(), maxLinearError/sqrt(maxLinearLink->transVariance()));
 					if(maxLinearErrorRatio > _optimizationMaxError)
 					{
-						UWARN("Rejecting all added loop closures (%d) in this "
+						UWARN("Rejecting all added loop closures (%d, first is %d <-> %d) in this "
 							  "iteration because a wrong loop closure has been "
 							  "detected after graph optimization, resulting in "
 							  "a maximum graph error ratio of %f (edge %d->%d, type=%d, abs error=%f m, stddev=%f). The "
 							  "maximum error ratio parameter \"%s\" is %f of std deviation.",
 							  (int)loopClosureLinksAdded.size(),
+							  loopClosureLinksAdded.front().first,
+							  loopClosureLinksAdded.front().second,
 							  maxLinearErrorRatio,
 							  maxLinearLink->from(),
 							  maxLinearLink->to(),
@@ -2392,12 +2394,14 @@ bool Rtabmap::process(
 					UINFO("Max optimization angular error = %f deg (link %d->%d, var=%f, ratio error/std=%f)", maxAngularError*180.0f/CV_PI, maxAngularLink->from(), maxAngularLink->to(), maxAngularLink->rotVariance(), maxAngularError/sqrt(maxAngularLink->rotVariance()));
 					if(maxAngularErrorRatio > _optimizationMaxError)
 					{
-						UWARN("Rejecting all added loop closures (%d) in this "
+						UWARN("Rejecting all added loop closures (%d, first is %d <-> %d) in this "
 							  "iteration because a wrong loop closure has been "
 							  "detected after graph optimization, resulting in "
 							  "a maximum graph error ratio of %f (edge %d->%d, type=%d, abs error=%f deg, stddev=%f). The "
 							  "maximum error ratio parameter \"%s\" is %f of std deviation.",
 							  (int)loopClosureLinksAdded.size(),
+							  loopClosureLinksAdded.front().first,
+							  loopClosureLinksAdded.front().second,
 							  maxAngularErrorRatio,
 							  maxAngularLink->from(),
 							  maxAngularLink->to(),
@@ -2789,7 +2793,7 @@ bool Rtabmap::process(
 		std::map<int, Signature> signatures;
 		if(_publishLastSignatureData)
 		{
-			UINFO("Adding data %d (rgb/left=%d depth/right=%d)", lastSignatureData.id(), lastSignatureData.sensorData().imageRaw().empty()?0:1, lastSignatureData.sensorData().depthOrRightRaw().empty()?0:1);
+			UINFO("Adding data %d [%d] (rgb/left=%d depth/right=%d)", lastSignatureData.id(), lastSignatureData.mapId(), lastSignatureData.sensorData().imageRaw().empty()?0:1, lastSignatureData.sensorData().depthOrRightRaw().empty()?0:1);
 			signatures.insert(std::make_pair(lastSignatureData.id(), lastSignatureData));
 		}
 		UDEBUG("");
@@ -2811,6 +2815,11 @@ bool Rtabmap::process(
 		std::map<int, Transform> groundTruths;
 		for(std::map<int, Transform>::iterator iter=poses.begin(); iter!=poses.end(); ++iter)
 		{
+			if(_publishLastSignatureData && lastSignatureData.id() == iter->first)
+			{
+				//already added
+				continue;
+			}
 			Transform odomPoseLocal;
 			int weight = -1;
 			int mapId = -1;
@@ -3486,26 +3495,26 @@ std::map<int, Transform> Rtabmap::optimizeGraph(
 	{
 		optimizedPoses = _graphOptimizer->optimize(fromId, poses, edgeConstraints, covariance, 0, error, iterationsDone);
 
-		if(!poses.empty() && optimizedPoses.empty() && guessPoses.empty())
+		if(!poses.empty() && optimizedPoses.empty())
 		{
-			UWARN("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);
+			UWARN("Optimization has failed, trying multi-session optimization instead (poses=%d, guess=%d, links=%d)...", (int)poses.size(), (int)guessPoses.size(), (int)edgeConstraints.size());
+			optimizedPoses = _graphOptimizer->optimizeMultiSession(fromId, poses, edgeConstraints, 0, error, iterationsDone);
 
 			if(optimizedPoses.empty())
 			{
 				if(!_graphOptimizer->isCovarianceIgnored() || _graphOptimizer->type() != Optimizer::kTypeTORO)
 				{
-					UWARN("Incremental optimization also failed. You may try changing parameters to %s=0 and %s=true.",
+					UWARN("Multi-session optimization also failed. You may try changing parameters to %s=0 and %s=true.",
 							Parameters::kOptimizerStrategy().c_str(), Parameters::kOptimizerVarianceIgnored().c_str());
 				}
 				else
 				{
-					UWARN("Incremental optimization also failed.");
+					UWARN("Multi-session optimization also failed.");
 				}
 			}
 			else
 			{
-				UWARN("Incremental optimization succeeded!");
+				UWARN("Multi-session optimization succeeded!");
 			}
 		}
 	}

corelib/src/optimizer/OptimizerG2O.cpp

@@ -1192,7 +1192,7 @@ std::map<int, Transform> OptimizerG2O::optimizeBA(
 
 		UASSERT(optimizer.verifyInformationMatrices());
 
-		UINFO("g2o optimizing begin (max iterations=%d, robustKernel=%f)", iterations(), robustKernelDelta_);
+		UDEBUG("g2o optimizing begin (max iterations=%d, robustKernel=%f)", iterations(), robustKernelDelta_);
 
 		int it = 0;
 		UTimer timer;
@@ -1270,7 +1270,7 @@ std::map<int, Transform> OptimizerG2O::optimizeBA(
 				UDEBUG("outliers=%d outliersCountFar=%d", outliersCount, outliersCountFar);
 			}
 		}
-		UINFO("g2o optimizing end (%d iterations done, error=%f, outliers=%d/%d (delta=%f) time = %f s)", it, optimizer.activeRobustChi2(), outliersCount, (int)edges.size(), robustKernelDelta_, timer.ticks());
+		UDEBUG("g2o optimizing end (%d iterations done, error=%f, outliers=%d/%d (delta=%f) time = %f s)", it, optimizer.activeRobustChi2(), outliersCount, (int)edges.size(), robustKernelDelta_, timer.ticks());
 
 		if(optimizer.activeRobustChi2() > 1000000000000.0)
 		{

corelib/src/optimizer/OptimizerGTSAM.cpp

@@ -310,7 +310,7 @@ std::map<int, Transform> OptimizerGTSAM::optimize(
 			optimizer = new gtsam::LevenbergMarquardtOptimizer(graph, initialEstimate, parameters);
 		}
 
-		UINFO("GTSAM optimizing begin (max iterations=%d, robust=%d)", iterations(), isRobust()?1:0);
+		UDEBUG("GTSAM optimizing begin (max iterations=%d, robust=%d)", iterations(), isRobust()?1:0);
 		UTimer timer;
 		int it = 0;
 		double lastError = optimizer->error();
@@ -344,7 +344,9 @@ std::map<int, Transform> OptimizerGTSAM::optimize(
 			}
 			catch(gtsam::IndeterminantLinearSystemException & e)
 			{
-				UWARN("GTSAM exception caught: %s", e.what());
+				UWARN("GTSAM exception caught: %s\n Graph has %d edges and %d vertices", e.what(),
+						(int)edgeConstraints.size(),
+						(int)poses.size());
 				delete optimizer;
 				return optimizedPoses;
 			}
@@ -361,7 +363,7 @@ std::map<int, Transform> OptimizerGTSAM::optimize(
 				}
 				else
 				{
-					UINFO("Stop optimizing, not enough improvement (%f < %f)", errorDelta, this->epsilon());
+					UDEBUG("Stop optimizing, not enough improvement (%f < %f)", errorDelta, this->epsilon());
 					break;
 				}
 			}
@@ -380,7 +382,7 @@ std::map<int, Transform> OptimizerGTSAM::optimize(
 		{
 			*iterationsDone = it;
 		}
-		UINFO("GTSAM optimizing end (%d iterations done, error=%f (initial=%f final=%f), time=%f s)",
+		UDEBUG("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());
 
 		gtsam::Marginals marginals(graph, optimizer->values());
@@ -406,10 +408,9 @@ std::map<int, Transform> OptimizerGTSAM::optimize(
 			UTimer t;
 			gtsam::Marginals marginals(graph, optimizer->values());
 			gtsam::Matrix info = marginals.marginalCovariance(optimizer->values().rbegin()->key);
-			UINFO("Computed marginals = %fs (key=%d)", t.ticks(), optimizer->values().rbegin()->key);
-			if(isSlam2d())
+			UDEBUG("Computed marginals = %fs (key=%d)", t.ticks(), optimizer->values().rbegin()->key);
+			if(isSlam2d() && info.cols() == 3 && info.cols() == 3)
 			{
-				UASSERT(info.cols() == 3 && info.cols() == 3);
 				outputCovariance.at<double>(0,0) = info(0,0); // x-x
 				outputCovariance.at<double>(0,1) = info(0,1); // x-y
 				outputCovariance.at<double>(0,5) = info(0,2); // x-theta
@@ -420,9 +421,8 @@ std::map<int, Transform> OptimizerGTSAM::optimize(
 				outputCovariance.at<double>(5,1) = info(2,1); // theta-y
 				outputCovariance.at<double>(5,5) = info(2,2); // theta-theta
 			}
-			else
+			else if(!isSlam2d() && info.cols() == 6 && info.cols() == 6)
 			{
-				UASSERT(info.cols() == 6 && info.cols() == 6);
 				Eigen::Matrix<double, 6, 6> mgtsam = Eigen::Matrix<double, 6, 6>::Identity();
 				mgtsam.block(3,3,3,3) = info.block(0,0,3,3); // cov rotation
 				mgtsam.block(0,0,3,3) = info.block(3,3,3,3); // cov translation
@@ -430,8 +430,18 @@ std::map<int, Transform> OptimizerGTSAM::optimize(
 				mgtsam.block(3,0,3,3) = info.block(3,0,3,3); // off diagonal
 				memcpy(outputCovariance.data, mgtsam.data(), outputCovariance.total()*sizeof(double));
 			}
-		} catch(std::exception& e) {
-			cout << e.what() << endl;
+			else
+			{
+				UWARN("GTSAM: Could not compute marginal covariance!");
+			}
+		}
+		catch(gtsam::IndeterminantLinearSystemException & e)
+		{
+			UWARN("GTSAM exception caught: %s", e.what());
+		}
+		catch(std::exception& e)
+		{
+			UWARN("GTSAM exception caught: %s", e.what());
 		}
 
 		delete optimizer;

guilib/include/rtabmap/gui/DatabaseViewer.h

@@ -114,6 +114,8 @@ private Q_SLOTS:
 	void view3DMap();
 	void generate3DMap();
 	void detectMoreLoopClosures();
+	void updateAllNeighborCovariances();
+	void updateAllLoopClosureCovariances();
 	void refineAllNeighborLinks();
 	void refineAllLoopClosureLinks();
 	void resetAllChanges();
@@ -174,6 +176,8 @@ private:
 	std::multimap<int, rtabmap::Link> updateLinksWithModifications(
 			const std::multimap<int, rtabmap::Link> & edgeConstraints);
 	void updateLoopClosuresSlider(int from = 0, int to = 0);
+	void updateAllCovariances(const QList<Link> & links);
+	void refineAllLinks(const QList<Link> & links);
 	void refineConstraint(int from, int to,  bool silent);
 	bool addConstraint(int from, int to, bool silent);
 	void exportPoses(int format);
@@ -198,6 +202,7 @@ private:
 	std::list<std::map<int, rtabmap::Transform> > graphes_;
 	std::multimap<int, rtabmap::Link> graphLinks_;
 	std::map<int, rtabmap::Transform> odomPoses_;
+	std::map<int, rtabmap::Transform> dbOptimizedPoses_;
 	std::map<int, rtabmap::Transform> groundTruthPoses_;
 	std::map<int, rtabmap::Transform> gpsPoses_;
 	std::map<int, GPS> gpsValues_;

guilib/src/DatabaseViewer.cpp

@@ -256,6 +256,8 @@ DatabaseViewer::DatabaseViewer(const QString & ini, QWidget * parent) :
 	connect(ui_->actionView_3D_map, SIGNAL(triggered()), this, SLOT(view3DMap()));
 	connect(ui_->actionGenerate_3D_map_pcd, SIGNAL(triggered()), this, SLOT(generate3DMap()));
 	connect(ui_->actionDetect_more_loop_closures, SIGNAL(triggered()), this, SLOT(detectMoreLoopClosures()));
+	connect(ui_->actionUpdate_all_neighbor_covariances, SIGNAL(triggered()), this, SLOT(updateAllNeighborCovariances()));
+	connect(ui_->actionUpdate_all_loop_closure_covariances, SIGNAL(triggered()), this, SLOT(updateAllLoopClosureCovariances()));
 	connect(ui_->actionRefine_all_neighbor_links, SIGNAL(triggered()), this, SLOT(refineAllNeighborLinks()));
 	connect(ui_->actionRefine_all_loop_closure_links, SIGNAL(triggered()), this, SLOT(refineAllLoopClosureLinks()));
 	connect(ui_->actionRegenerate_local_grid_maps, SIGNAL(triggered()), this, SLOT(regenerateLocalMaps()));
@@ -1466,6 +1468,7 @@ void DatabaseViewer::updateIds()
 	weights_.clear();
 	wmStates_.clear();
 	odomPoses_.clear();
+	dbOptimizedPoses_.clear();
 	groundTruthPoses_.clear();
 	gpsPoses_.clear();
 	gpsValues_.clear();
@@ -1626,6 +1629,8 @@ void DatabaseViewer::updateIds()
 		}
 	}
 
+	dbOptimizedPoses_ = dbDriver_->loadOptimizedPoses();
+
 	if(!groundTruthPoses_.empty() || !gpsPoses_.empty())
 	{
 		ui_->checkBox_alignPosesWithGroundTruth->setVisible(true);
@@ -3290,24 +3295,75 @@ void DatabaseViewer::detectMoreLoopClosures()
 	progressDialog->setValue(progressDialog->maximumSteps());
 }
 
-void DatabaseViewer::refineAllNeighborLinks()
+void DatabaseViewer::updateAllNeighborCovariances()
 {
-	if(neighborLinks_.size())
+	updateAllCovariances(neighborLinks_);
+}
+void DatabaseViewer::updateAllLoopClosureCovariances()
+{
+	updateAllCovariances(loopLinks_);
+}
+
+void DatabaseViewer::updateAllCovariances(const QList<Link> & links)
+{
+	if(links.size())
 	{
+		bool ok = false;
+		double stddev = QInputDialog::getDouble(this, tr("Linear error"), tr("Std deviation (m)"), 0.01, 0.0001, 9, 4, &ok);
+		if(!ok) return;
+		double linearVar = stddev*stddev;
+		stddev = QInputDialog::getDouble(this, tr("Angular error"), tr("Std deviation (deg)"), 1, 0.01, 45, 2, &ok)*M_PI/180.0;
+		if(!ok) return;
+		double angularVar = stddev*stddev;
+
 		rtabmap::ProgressDialog * progressDialog = new rtabmap::ProgressDialog(this);
 		progressDialog->setAttribute(Qt::WA_DeleteOnClose);
-		progressDialog->setMaximumSteps(neighborLinks_.size());
+		progressDialog->setMaximumSteps(links.size());
 		progressDialog->setCancelButtonVisible(true);
 		progressDialog->setMinimumWidth(800);
 		progressDialog->show();
 
-		for(int i=0; i<neighborLinks_.size(); ++i)
-		{
-			int from = neighborLinks_[i].from();
-			int to = neighborLinks_[i].to();
-			this->refineConstraint(neighborLinks_[i].from(), neighborLinks_[i].to(), true);
+		cv::Mat infMatrix = cv::Mat::eye(6,6,CV_64FC1);
+		infMatrix(cv::Range(0,3), cv::Range(0,3))/=linearVar;
+		infMatrix(cv::Range(3,6), cv::Range(3,6))/=angularVar;
 
-			progressDialog->appendText(tr("Refined link %1->%2 (%3/%4)").arg(from).arg(to).arg(i+1).arg(neighborLinks_.size()));
+		for(int i=0; i<links.size(); ++i)
+		{
+			int from = links[i].from();
+			int to = links[i].to();
+
+			Link currentLink =  findActiveLink(from, to);
+			if(!currentLink.isValid())
+			{
+				UERROR("Not found link! (%d->%d)", from, to);
+				return;
+			}
+			currentLink = Link(
+					currentLink.from(),
+					currentLink.to(),
+					currentLink.type(),
+					currentLink.transform(),
+					infMatrix.clone(),
+					currentLink.userDataCompressed());
+			bool updated = false;
+			std::multimap<int, Link>::iterator iter = linksRefined_.find(currentLink.from());
+			while(iter != linksRefined_.end() && iter->first == currentLink.from())
+			{
+				if(iter->second.to() == currentLink.to() &&
+				   iter->second.type() == currentLink.type())
+				{
+					iter->second = currentLink;
+					updated = true;
+					break;
+				}
+				++iter;
+			}
+			if(!updated)
+			{
+				linksRefined_.insert(std::make_pair(currentLink.from(), currentLink));
+			}
+
+			progressDialog->appendText(tr("Updated link %1->%2 (%3/%4)").arg(from).arg(to).arg(i+1).arg(links.size()));
 			progressDialog->incrementStep();
 			QApplication::processEvents();
 			if(progressDialog->isCanceled())
@@ -3322,24 +3378,32 @@ void DatabaseViewer::refineAllNeighborLinks()
 	}
 }
 
+void DatabaseViewer::refineAllNeighborLinks()
+{
+	refineAllLinks(neighborLinks_);
+}
 void DatabaseViewer::refineAllLoopClosureLinks()
 {
-	if(loopLinks_.size())
+	refineAllLinks(loopLinks_);
+}
+void DatabaseViewer::refineAllLinks(const QList<Link> & links)
+{
+	if(links.size())
 	{
 		rtabmap::ProgressDialog * progressDialog = new rtabmap::ProgressDialog(this);
 		progressDialog->setAttribute(Qt::WA_DeleteOnClose);
-		progressDialog->setMaximumSteps(loopLinks_.size());
+		progressDialog->setMaximumSteps(links.size());
 		progressDialog->setCancelButtonVisible(true);
 		progressDialog->setMinimumWidth(800);
 		progressDialog->show();
 
-		for(int i=0; i<loopLinks_.size(); ++i)
+		for(int i=0; i<links.size(); ++i)
 		{
-			int from = loopLinks_[i].from();
-			int to = loopLinks_[i].to();
-			this->refineConstraint(loopLinks_[i].from(), loopLinks_[i].to(), true);
+			int from = links[i].from();
+			int to = links[i].to();
+			this->refineConstraint(links[i].from(), links[i].to(), true);
 
-			progressDialog->appendText(tr("Refined link %1->%2 (%3/%4)").arg(from).arg(to).arg(i+1).arg(loopLinks_.size()));
+			progressDialog->appendText(tr("Refined link %1->%2 (%3/%4)").arg(from).arg(to).arg(i+1).arg(links.size()));
 			progressDialog->incrementStep();
 			QApplication::processEvents();
 			if(progressDialog->isCanceled())
@@ -5373,6 +5437,10 @@ void DatabaseViewer::updateGraphView()
 		{
 			optimizedGraphGuess = lastOptimizedGraph_;
 		}
+		else
+		{
+			optimizedGraphGuess = dbOptimizedPoses_;
+		}
 
 		graphes_.clear();
 		graphLinks_.clear();
@@ -5610,17 +5678,16 @@ void DatabaseViewer::updateGraphView()
 		time.start();
 		std::map<int, rtabmap::Transform> finalPoses = optimizer->optimize(fromId, posesOut, linksOut, ui_->checkBox_iterativeOptimization->isChecked()?&graphes_:0);
 		ui_->label_timeOptimization->setNum(double(time.elapsed())/1000.0);
-		graphes_.push_back(finalPoses);
 		graphLinks_ = linksOut;
 		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_);
+			UWARN("Optimization failed, trying multi-session optimization instead... (poses=%d, links=%d).", (int)posesOut.size(), (int)linksOut.size());
+			finalPoses = optimizer->optimizeMultiSession(fromId, posesOut, linksOut, &graphes_);
 
 			if(finalPoses.empty())
 			{
-				UWARN("Incremental optimization also failed.");
+				UWARN("Multi-session 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. "
@@ -5631,17 +5698,8 @@ void DatabaseViewer::updateGraphView()
 					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;
-				lastOptimizedGraph_ = finalPoses;
-
-			}
 		}
+		graphes_.push_back(finalPoses);
 		delete optimizer;
 	}
 	if(graphes_.size())
@@ -6308,11 +6366,29 @@ bool DatabaseViewer::addConstraint(int from, int to, bool silent)
 		UASSERT_MSG(odomPoses_.find(newLink.from()) != odomPoses_.end(), uFormat("id=%d poses=%d links=%d", newLink.from(), (int)poses.size(), (int)links.size()).c_str());
 		UASSERT_MSG(odomPoses_.find(newLink.to()) != odomPoses_.end(), uFormat("id=%d poses=%d links=%d", newLink.to(), (int)poses.size(), (int)links.size()).c_str());
 		optimizer->getConnectedGraph(fromId, odomPoses_, linksIn, poses, links);
+		// use already optimized poses
+		if(graphes_.size())
+		{
+			const std::map<int, Transform> & optimizedPoses = graphes_.back();
+			for(std::map<int, Transform>::iterator iter = poses.begin(); iter!=poses.end(); ++iter)
+			{
+				if(optimizedPoses.find(iter->first) != optimizedPoses.end())
+				{
+					iter->second = optimizedPoses.at(iter->first);
+				}
+			}
+		}
 		UASSERT(poses.find(fromId) != poses.end());
 		UASSERT_MSG(poses.find(newLink.from()) != poses.end(), uFormat("id=%d poses=%d links=%d", newLink.from(), (int)poses.size(), (int)links.size()).c_str());
 		UASSERT_MSG(poses.find(newLink.to()) != poses.end(), uFormat("id=%d poses=%d links=%d", newLink.to(), (int)poses.size(), (int)links.size()).c_str());
 		UASSERT(graph::findLink(links, newLink.from(), newLink.to()) != links.end());
-		poses = optimizer->optimize(fromId, poses, links);
+		std::map<int, Transform> posesIn = poses;
+		poses = optimizer->optimize(fromId, posesIn, links);
+		if(posesIn.size() && poses.empty())
+		{
+			UWARN("Optimization failed, trying multi-session optimization instead... (poses=%d, links=%d).", (int)posesIn.size(), (int)links.size());
+			poses = optimizer->optimizeMultiSession(fromId, posesIn, links);
+		}
 		std::string msg;
 		if(poses.size())
 		{
@@ -6333,7 +6409,7 @@ bool DatabaseViewer::addConstraint(int from, int to, bool silent)
 				if(maxLinearErrorRatio > maxOptimizationError)
 				{
 					msg = uFormat("Rejecting edge %d->%d because "
-						  "graph error is too large after optimization (ratio %f for edge %d->%d, stddev=%f). "
+						  "graph error is too large after optimization (ratio %f for edge %d->%d, stddev=%f m). "
 						  "\"%s\" is %f.",
 						  newLink.from(),
 						  newLink.to(),
@@ -6351,7 +6427,7 @@ bool DatabaseViewer::addConstraint(int from, int to, bool silent)
 				if(maxAngularErrorRatio > maxOptimizationError)
 				{
 					msg = uFormat("Rejecting edge %d->%d because "
-						  "graph error is too large after optimization (ratio %f for edge %d->%d, stddev=%f). "
+						  "graph error is too large after optimization (ratio %f for edge %d->%d, stddev=%f deg). "
 						  "\"%s\" is %f.",
 						  newLink.from(),
 						  newLink.to(),

guilib/src/ui/DatabaseViewer.ui

@@ -61,8 +61,8 @@
           <rect>
            <x>0</x>
            <y>0</y>
-           <width>327</width>
-           <height>222</height>
+           <width>301</width>
+           <height>242</height>
           </rect>
          </property>
          <layout class="QGridLayout" name="gridLayout" columnstretch="0,1">
@@ -253,8 +253,8 @@
           <rect>
            <x>0</x>
            <y>0</y>
-           <width>327</width>
-           <height>222</height>
+           <width>301</width>
+           <height>242</height>
           </rect>
          </property>
          <layout class="QGridLayout" name="gridLayout_2" columnstretch="0,1">
@@ -579,7 +579,7 @@
      <x>0</x>
      <y>0</y>
      <width>1547</width>
-     <height>25</height>
+     <height>22</height>
     </rect>
    </property>
    <widget class="QMenu" name="menuFile">
@@ -637,6 +637,8 @@
     <addaction name="actionDetect_more_loop_closures"/>
     <addaction name="actionRefine_all_neighbor_links"/>
     <addaction name="actionRefine_all_loop_closure_links"/>
+    <addaction name="actionUpdate_all_neighbor_covariances"/>
+    <addaction name="actionUpdate_all_loop_closure_covariances"/>
     <addaction name="separator"/>
     <addaction name="actionRegenerate_local_grid_maps"/>
     <addaction name="actionRegenerate_local_grid_maps_selected"/>
@@ -1228,8 +1230,8 @@
          <rect>
           <x>0</x>
           <y>0</y>
-          <width>312</width>
-          <height>200</height>
+          <width>318</width>
+          <height>197</height>
          </rect>
         </property>
         <attribute name="label">
@@ -1389,9 +1391,9 @@
         <property name="geometry">
          <rect>
           <x>0</x>
-          <y>-167</y>
-          <width>441</width>
-          <height>764</height>
+          <y>0</y>
+          <width>519</width>
+          <height>791</height>
          </rect>
         </property>
         <attribute name="label">
@@ -1924,8 +1926,8 @@
          <rect>
           <x>0</x>
           <y>0</y>
-          <width>200</width>
-          <height>111</height>
+          <width>205</width>
+          <height>114</height>
          </rect>
         </property>
         <attribute name="label">
@@ -2024,8 +2026,8 @@
          <rect>
           <x>0</x>
           <y>0</y>
-          <width>179</width>
-          <height>452</height>
+          <width>185</width>
+          <height>485</height>
          </rect>
         </property>
         <attribute name="label">
@@ -2696,6 +2698,16 @@
     <string>Update optimized mesh...</string>
    </property>
   </action>
+  <action name="actionUpdate_all_neighbor_covariances">
+   <property name="text">
+    <string>Update all neighbor covariances...</string>
+   </property>
+  </action>
+  <action name="actionUpdate_all_loop_closure_covariances">
+   <property name="text">
+    <string>Update all loop closure covariances...</string>
+   </property>
+  </action>
  </widget>
  <customwidgets>
   <customwidget>

tools/Reprocess/main.cpp

@@ -240,6 +240,7 @@ int main(int argc, char * argv[])
 	std::string workingDirectory = UDirectory::getDir(outputDatabasePath);
 	printf("Set working directory to \"%s\".\n", workingDirectory.c_str());
 	uInsert(parameters, ParametersPair(Parameters::kRtabmapWorkingDirectory(), workingDirectory));
+	uInsert(parameters, ParametersPair(Parameters::kRtabmapPublishStats(), "true")); // to log status below
 
 	Rtabmap rtabmap;
 	rtabmap.init(parameters, outputDatabasePath);
@@ -369,11 +370,11 @@ int main(int argc, char * argv[])
 		if (loopId>0)
 		{
 			int loopMapId = uContains(stats.getSignatures(), loopId) ? stats.getSignatures().at(loopId).mapId() : -1;
-			printf("Processed %d/%d nodes [Map=%d]... %dms Loop on %d [Map=%d]\n", ++processed, totalIds, loopMapId, int(iterationTime.ticks() * 1000), loopId, loopMapId);
+			printf("Processed %d/%d nodes [%d]... %dms Loop on %d [%d]\n", ++processed, totalIds, refMapId, int(iterationTime.ticks() * 1000), loopId, loopMapId);
 		}
 		else
 		{
-			printf("Processed %d/%d nodes [Map=%d]... %dms\n", ++processed, totalIds, refMapId, int(iterationTime.ticks() * 1000));
+			printf("Processed %d/%d nodes [%d]... %dms\n", ++processed, totalIds, refMapId, int(iterationTime.ticks() * 1000));
 		}
 
 		data = dbReader.takeImage(&info);