0.17.2: compute marginals (covariance) on graph optimization

CMakeLists.txt

@@ -21,7 +21,7 @@ SET(CMAKE_MODULE_PATH "${PROJECT_SOURCE_DIR}/cmake_modules")
 #######################
 SET(RTABMAP_MAJOR_VERSION 0)
 SET(RTABMAP_MINOR_VERSION 17)
-SET(RTABMAP_PATCH_VERSION 1)
+SET(RTABMAP_PATCH_VERSION 2)
 SET(RTABMAP_VERSION
   ${RTABMAP_MAJOR_VERSION}.${RTABMAP_MINOR_VERSION}.${RTABMAP_PATCH_VERSION})
   

corelib/include/rtabmap/core/Optimizer.h

@@ -95,14 +95,23 @@ public:
 			double * finalError = 0,
 			int * iterationsDone = 0);
 
+	std::map<int, Transform> optimize(
+				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,
+				int rootId,
-			const std::map<int, Transform> & poses,
+				const std::map<int, Transform> & poses,
-			const std::multimap<int, Link> & constraints,
+				const std::multimap<int, Link> & constraints,
-			std::list<std::map<int, Transform> > * intermediateGraphes = 0,
+				cv::Mat & outputCovariance,
-			double * finalError = 0,
+				std::list<std::map<int, Transform> > * intermediateGraphes = 0,
-			int * iterationsDone = 0);
+				double * finalError = 0,
+				int * iterationsDone = 0);
 	virtual std::map<int, Transform> optimizeBA(
 			int rootId, // if negative, all other poses are fixed
 			const std::map<int, Transform> & poses,

corelib/include/rtabmap/core/OptimizerG2O.h

@@ -64,12 +64,13 @@ public:
 	virtual void parseParameters(const ParametersMap & parameters);
 
 	virtual std::map<int, Transform> optimize(
-			int rootId,
+				int rootId,
-			const std::map<int, Transform> & poses,
+				const std::map<int, Transform> & poses,
-			const std::multimap<int, Link> & edgeConstraints,
+				const std::multimap<int, Link> & edgeConstraints,
-			std::list<std::map<int, Transform> > * intermediateGraphes = 0,
+				cv::Mat & outputCovariance,
-			double * finalError = 0,
+				std::list<std::map<int, Transform> > * intermediateGraphes = 0,
-			int * iterationsDone = 0);
+				double * finalError = 0,
+				int * iterationsDone = 0);
 
 	virtual std::map<int, Transform> optimizeBA(
 			int rootId,

corelib/include/rtabmap/core/OptimizerGTSAM.h

@@ -56,6 +56,7 @@ public:
 			int rootId,
 			const std::map<int, Transform> & poses,
 			const std::multimap<int, Link> & edgeConstraints,
+			cv::Mat & outputCovariance,
 			std::list<std::map<int, Transform> > * intermediateGraphes = 0,
 			double * finalError = 0,
 			int * iterationsDone = 0);

corelib/include/rtabmap/core/OptimizerTORO.h

@@ -66,6 +66,7 @@ public:
 			int rootId,
 			const std::map<int, Transform> & poses,
 			const std::multimap<int, Link> & edgeConstraints,
+			cv::Mat & outputCovariance,
 			std::list<std::map<int, Transform> > * intermediateGraphes = 0,
 			double * finalError = 0,
 			int * iterationsDone = 0);

corelib/include/rtabmap/core/Rtabmap.h

@@ -190,6 +190,7 @@ private:
 	void optimizeCurrentMap(int id,
 			bool lookInDatabase,
 			std::map<int, Transform> & optimizedPoses,
+			cv::Mat & covariance,
 			std::multimap<int, Link> * constraints = 0,
 			double * error = 0,
 			int * iterationsDone = 0) const;
@@ -198,6 +199,7 @@ private:
 			const std::set<int> & ids,
 			const std::map<int, Transform> & guessPoses,
 			bool lookInDatabase,
+			cv::Mat & covariance,
 			std::multimap<int, Link> * constraints = 0,
 			double * error = 0,
 			int * iterationsDone = 0) const;

corelib/include/rtabmap/core/Statistics.h

@@ -183,6 +183,7 @@ public:
 	void setConstraints(const std::multimap<int, Link> & constraints) {_constraints = constraints;}
 	void setMapCorrection(const Transform & mapCorrection) {_mapCorrection = mapCorrection;}
 	void setLoopClosureTransform(const Transform & loopClosureTransform) {_loopClosureTransform = loopClosureTransform;}
+	void setLocalizationCovariance(const cv::Mat & covariance) {_localizationCovariance = covariance;}
 	void setWeights(const std::map<int, int> & weights) {_weights = weights;}
 	void setPosterior(const std::map<int, float> & posterior) {_posterior = posterior;}
 	void setLikelihood(const std::map<int, float> & likelihood) {_likelihood = likelihood;}
@@ -205,6 +206,7 @@ public:
 	const std::multimap<int, Link> & constraints() const {return _constraints;}
 	const Transform & mapCorrection() const {return _mapCorrection;}
 	const Transform & loopClosureTransform() const {return _loopClosureTransform;}
+	const cv::Mat & localizationCovariance() const {return _localizationCovariance;}
 	const std::map<int, int> & weights() const {return _weights;}
 	const std::map<int, float> & posterior() const {return _posterior;}
 	const std::map<int, float> & likelihood() const {return _likelihood;}
@@ -230,6 +232,7 @@ private:
 	std::multimap<int, Link> _constraints;
 	Transform _mapCorrection;
 	Transform _loopClosureTransform;
+	cv::Mat _localizationCovariance;
 
 	std::map<int, int> _weights;
 	std::map<int, float> _posterior;

corelib/src/Optimizer.cpp

@@ -328,10 +328,29 @@ std::map<int, Transform> Optimizer::optimizeIncremental(
 	return std::map<int, Transform>();
 }
 
+std::map<int, Transform> Optimizer::optimize(
+		int rootId,
+		const std::map<int, Transform> & poses,
+		const std::multimap<int, Link> & edgeConstraints,
+		std::list<std::map<int, Transform> > * intermediateGraphes,
+		double * finalError,
+		int * iterationsDone)
+{
+	cv::Mat covariance;
+	return optimize(rootId,
+			poses,
+			edgeConstraints,
+			covariance,
+			intermediateGraphes,
+			finalError,
+			iterationsDone);
+}
+
 std::map<int, Transform> Optimizer::optimize(
 		int rootId,
 		const std::map<int, Transform> & poses,
 		const std::multimap<int, Link> & constraints,
+		cv::Mat & outputCovariance,
 		std::list<std::map<int, Transform> > * intermediateGraphes,
 		double * finalError,
 		int * iterationsDone)

corelib/src/OptimizerG2O.cpp

@@ -164,6 +164,7 @@ std::map<int, Transform> OptimizerG2O::optimize(
 		int rootId,
 		const std::map<int, Transform> & poses,
 		const std::multimap<int, Link> & edgeConstraints,
+		cv::Mat & outputCovariance,
 		std::list<std::map<int, Transform> > * intermediateGraphes,
 		double * finalError,
 		int * iterationsDone)
@@ -659,6 +660,31 @@ std::map<int, Transform> OptimizerG2O::optimize(
 					UERROR("Vertex %d not found!?", iter->first);
 				}
 			}
+
+			g2o::VertexSE2* v = (g2o::VertexSE2*)optimizer.vertex(poses.rbegin()->first);
+			if(v)
+			{
+				UTimer t;
+				g2o::SparseBlockMatrix<g2o::MatrixXD> spinv;
+				optimizer.computeMarginals(spinv, v);
+				UINFO("Computed marginals = %fs (cols=%d rows=%d, v=%d id=%d)", t.ticks(), spinv.cols(), spinv.rows(), v->hessianIndex(), poses.rbegin()->first);
+				g2o::SparseBlockMatrix<g2o::MatrixXD>::SparseMatrixBlock * block = spinv.blockCols()[v->hessianIndex()].begin()->second;
+				UASSERT(block && block->cols() == 3 && block->cols() == 3);
+				outputCovariance = cv::Mat::eye(6,6,CV_64FC1);
+				outputCovariance.at<double>(0,0) = (*block)(0,0); // x-x
+				outputCovariance.at<double>(0,1) = (*block)(0,1); // x-y
+				outputCovariance.at<double>(0,5) = (*block)(0,2); // x-theta
+				outputCovariance.at<double>(1,0) = (*block)(1,0); // y-x
+				outputCovariance.at<double>(1,1) = (*block)(1,1); // y-y
+				outputCovariance.at<double>(1,5) = (*block)(1,2); // y-theta
+				outputCovariance.at<double>(5,0) = (*block)(2,0); // theta-x
+				outputCovariance.at<double>(5,1) = (*block)(2,1); // theta-y
+				outputCovariance.at<double>(5,5) = (*block)(2,2); // theta-theta
+			}
+			else
+			{
+				UERROR("Vertex %d not found!? Cannot compute marginals...", poses.rbegin()->first);
+			}
 		}
 		else
 		{
@@ -676,6 +702,23 @@ std::map<int, Transform> OptimizerG2O::optimize(
 					UERROR("Vertex %d not found!?", iter->first);
 				}
 			}
+
+			g2o::VertexSE3* v = (g2o::VertexSE3*)optimizer.vertex(poses.rbegin()->first);
+			if(v)
+			{
+				UTimer t;
+				g2o::SparseBlockMatrix<g2o::MatrixXD> spinv;
+				optimizer.computeMarginals(spinv, v);
+				UINFO("Computed marginals = %fs (cols=%d rows=%d, v=%d id=%d)", t.ticks(), spinv.cols(), spinv.rows(), v->hessianIndex(), poses.rbegin()->first);
+				g2o::SparseBlockMatrix<g2o::MatrixXD>::SparseMatrixBlock * block = spinv.blockCols()[v->hessianIndex()].begin()->second;
+				UASSERT(block && block->cols() == 6 && block->cols() == 6);
+				outputCovariance = cv::Mat(6,6,CV_64FC1);
+				memcpy(outputCovariance.data, block->data(), outputCovariance.total()*sizeof(double));
+			}
+			else
+			{
+				UERROR("Vertex %d not found!? Cannot compute marginals...", poses.rbegin()->first);
+			}
 		}
 	}
 	else if(poses.size() == 1 || iterations() <= 0)

corelib/src/OptimizerGTSAM.cpp

@@ -79,6 +79,7 @@ std::map<int, Transform> OptimizerGTSAM::optimize(
 		int rootId,
 		const std::map<int, Transform> & poses,
 		const std::multimap<int, Link> & edgeConstraints,
+		cv::Mat & outputCovariance,
 		std::list<std::map<int, Transform> > * intermediateGraphes,
 		double * finalError,
 		int * iterationsDone)
@@ -381,6 +382,7 @@ std::map<int, Transform> OptimizerGTSAM::optimize(
 		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());
 
+		gtsam::Marginals marginals(graph, optimizer->values());
 		for(gtsam::Values::const_iterator iter=optimizer->values().begin(); iter!=optimizer->values().end(); ++iter)
 		{
 			if(iter->value.dim() > 1)
@@ -397,6 +399,42 @@ std::map<int, Transform> OptimizerGTSAM::optimize(
 				}
 			}
 		}
+
+		// compute marginals
+		try {
+			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())
+			{
+				UASSERT(info.cols() == 3 && info.cols() == 3);
+				outputCovariance = cv::Mat::eye(6,6,CV_64FC1);
+				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
+				outputCovariance.at<double>(1,0) = info(1,0); // y-x
+				outputCovariance.at<double>(1,1) = info(1,1); // y-y
+				outputCovariance.at<double>(1,5) = info(1,2); // y-theta
+				outputCovariance.at<double>(5,0) = info(2,0); // theta-x
+				outputCovariance.at<double>(5,1) = info(2,1); // theta-y
+				outputCovariance.at<double>(5,5) = info(2,2); // theta-theta
+			}
+			else
+			{
+				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
+				mgtsam.block(0,3,3,3) = info.block(0,3,3,3); // off diagonal
+				mgtsam.block(3,0,3,3) = info.block(3,0,3,3); // off diagonal
+				outputCovariance = cv::Mat(6,6,CV_64FC1);
+				memcpy(outputCovariance.data, mgtsam.data(), outputCovariance.total()*sizeof(double));
+			}
+		} catch(std::exception& e) {
+			cout << e.what() << endl;
+		}
+
 		delete optimizer;
 	}
 	else if(poses.size() == 1 || iterations() <= 0)

corelib/src/OptimizerTORO.cpp

@@ -55,6 +55,7 @@ std::map<int, Transform> OptimizerTORO::optimize(
 		int rootId,
 		const std::map<int, Transform> & poses,
 		const std::multimap<int, Link> & edgeConstraints,
+		cv::Mat & outputCovariance,
 		std::list<std::map<int, Transform> > * intermediateGraphes, // contains poses after tree init to last one before the end
 		double * finalError,
 		int * iterationsDone)
@@ -312,6 +313,9 @@ std::map<int, Transform> OptimizerTORO::optimize(
 				optimizedPoses.insert(std::pair<int, Transform>(iter->first, newPose));
 			}
 		}
+
+		// TORO doesn't compute marginals...
+		outputCovariance = cv::Mat::eye(6,6,CV_64FC1);
 	}
 	else if(poses.size() == 1 || iterations() <= 0)
 	{

corelib/src/Rtabmap.cpp

@@ -797,7 +797,8 @@ void Rtabmap::exportPoses(const std::string & path, bool optimized, bool global,
 
 		if(optimized)
 		{
-			this->optimizeCurrentMap(_memory->getLastWorkingSignature()->id(), global, poses, &constraints);
+			cv::Mat covariance;
+			this->optimizeCurrentMap(_memory->getLastWorkingSignature()->id(), global, poses, covariance, &constraints);
 		}
 		else
 		{
@@ -846,7 +847,8 @@ void Rtabmap::resetMemory()
 		_memory->init(_databasePath, true, _parameters, true);
 		if(_memory->getLastWorkingSignature())
 		{
-			optimizeCurrentMap(_memory->getLastWorkingSignature()->id(), false, _optimizedPoses, &_constraints);
+			cv::Mat covariance;
+			optimizeCurrentMap(_memory->getLastWorkingSignature()->id(), false, _optimizedPoses, covariance, &_constraints);
 		}
 		if(_bayesFilter)
 		{
@@ -2116,7 +2118,8 @@ bool Rtabmap::process(
 							if(_proximityRawPosesUsed)
 							{
 								//optimize the path's poses locally
-								path = optimizeGraph(nearestId, uKeysSet(path), std::map<int, Transform>(), false);
+								cv::Mat covariance;
+								path = optimizeGraph(nearestId, uKeysSet(path), std::map<int, Transform>(), false, covariance);
 								// transform local poses in optimized graph referential
 								UASSERT(uContains(path, nearestId));
 								Transform t = _optimizedPoses.at(nearestId) * path.at(nearestId).inverse();
@@ -2234,6 +2237,7 @@ bool Rtabmap::process(
 	float maxLinearErrorRatio = 0.0f;
 	double optimizationError = 0.0;
 	int optimizationIterations = 0;
+	cv::Mat localizationCovariance;
 	if(_rgbdSlamMode &&
 		(_loopClosureHypothesis.first>0 ||
 	     lastProximitySpaceClosureId>0 || // can be different map of the current one
@@ -2282,6 +2286,7 @@ bool Rtabmap::process(
 			{
 				_optimizedPoses.at(signature->id()) = _optimizedPoses.at(localizationLinks.begin()->first) * localizationLinks.begin()->second.transform().inverse();
 			}
+			localizationCovariance = localizationLinks.begin()->second.infMatrix().inv();
 		}
 		else
 		{
@@ -2306,7 +2311,8 @@ bool Rtabmap::process(
 			}
 
 			std::multimap<int, Link> constraints;
-			optimizeCurrentMap(signature->id(), false, poses, &constraints, &optimizationError, &optimizationIterations);
+			cv::Mat covariance;
+			optimizeCurrentMap(signature->id(), false, poses, covariance, &constraints, &optimizationError, &optimizationIterations);
 
 			// Check added loop closures have broken the graph
 			// (in case of wrong loop closures).
@@ -2389,6 +2395,7 @@ bool Rtabmap::process(
 				UINFO("Updated local map (old size=%d, new size=%d)", (int)_optimizedPoses.size(), (int)poses.size());
 				_optimizedPoses = poses;
 				_constraints = constraints;
+				localizationCovariance = covariance;
 			}
 		}
 
@@ -2479,6 +2486,7 @@ bool Rtabmap::process(
 			}
 			statistics_.setMapCorrection(_mapCorrection);
 			UINFO("Set map correction = %s", _mapCorrection.prettyPrint().c_str());
+			statistics_.setLocalizationCovariance(localizationCovariance);
 
 			// timings...
 			statistics_.addStatistic(Statistics::kTimingMemory_update(), timeMemoryUpdate*1000);
@@ -3221,6 +3229,7 @@ void Rtabmap::optimizeCurrentMap(
 		int id,
 		bool lookInDatabase,
 		std::map<int, Transform> & optimizedPoses,
+		cv::Mat & covariance,
 		std::multimap<int, Link> * constraints,
 		double * error,
 		int * iterationsDone) const
@@ -3237,7 +3246,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), optimizedPoses, lookInDatabase, constraints, error, iterationsDone);
+		std::map<int, Transform> poses = Rtabmap::optimizeGraph(id, uKeysSet(ids), optimizedPoses, lookInDatabase, covariance, constraints, error, iterationsDone);
 		UINFO("optimize time %f s", timer.ticks());
 
 		if(poses.size())
@@ -3267,6 +3276,7 @@ std::map<int, Transform> Rtabmap::optimizeGraph(
 		const std::set<int> & ids,
 		const std::map<int, Transform> & guessPoses,
 		bool lookInDatabase,
+		cv::Mat & covariance,
 		std::multimap<int, Link> * constraints,
 		double * error,
 		int * iterationsDone) const
@@ -3348,7 +3358,7 @@ std::map<int, Transform> Rtabmap::optimizeGraph(
 	}
 	else
 	{
-		optimizedPoses = _graphOptimizer->optimize(fromId, poses, edgeConstraints, 0, error, iterationsDone);
+		optimizedPoses = _graphOptimizer->optimize(fromId, poses, edgeConstraints, covariance, 0, error, iterationsDone);
 
 		if(!poses.empty() && optimizedPoses.empty() && guessPoses.empty())
 		{
@@ -3522,7 +3532,8 @@ void Rtabmap::get3DMap(
 			if(optimized)
 			{
 				poses = _optimizedPoses; // guess
-				this->optimizeCurrentMap(_memory->getLastWorkingSignature()->id(), global, poses, &constraints);
+				cv::Mat covariance;
+				this->optimizeCurrentMap(_memory->getLastWorkingSignature()->id(), global, poses, covariance, &constraints);
 			}
 			else
 			{
@@ -3609,7 +3620,8 @@ void Rtabmap::getGraph(
 			if(optimized)
 			{
 				poses = _optimizedPoses; // guess
-				this->optimizeCurrentMap(_memory->getLastWorkingSignature()->id(), global, poses, &constraints);
+				cv::Mat covariance;
+				this->optimizeCurrentMap(_memory->getLastWorkingSignature()->id(), global, poses, covariance, &constraints);
 			}
 			else
 			{

package.xml

@@ -1,7 +1,7 @@
 <?xml version="1.0"?>
 <package>
   <name>rtabmap</name>
-  <version>0.17.1</version>
+  <version>0.17.2</version>
   <description>RTAB-Map's standalone library. RTAB-Map is a RGB-D SLAM approach with real-time constraints.</description>
   <maintainer email="matlabbe@gmail.com">Mathieu Labbe</maintainer>
   <author>Mathieu Labbe</author>