Implemented OptimizerG2O::optimimzeBA(). Updated PostProcessingDialog (g2o sba option). Fixed words descriptors not filled in rtabmap::getMap3D(). CameraModelD(): return 5 null coeff distorsions if not set

cmake_modules/FindG2O.cmake

@@ -79,6 +79,7 @@ IF(G2O_STUFF_LIBRARY AND G2O_CORE_LIBRARY AND G2O_INCLUDE_DIR AND G2O_SOLVERS_FO
 	${G2O_CORE_LIBRARY}
 	${G2O_TYPES_SLAM2D} 
 	${G2O_TYPES_SLAM3D}
+	${G2O_TYPES_SBA}
         ${G2O_STUFF_LIBRARY})
   
   IF(CSPARSE_FOUND)

corelib/include/rtabmap/core/CameraModel.h

@@ -99,7 +99,7 @@ public:
 	cv::Mat K_raw() const {return K_;} //intrinsic camera matrix (before rectification)
 	cv::Mat D_raw() const {return D_;} //intrinsic distorsion matrix (before rectification)
 	cv::Mat K() const {return !P_.empty()?P_.colRange(0,3):K_;} // if P exists, return rectified version
-	cv::Mat D() const {return P_.empty()&&!D_.empty()?D_:cv::Mat::zeros(1,4,CV_64FC1);} // if P exists, return rectified version
+	cv::Mat D() const {return P_.empty()&&!D_.empty()?D_:cv::Mat::zeros(1,5,CV_64FC1);} // if P exists, return rectified version
 	cv::Mat R() const {return R_;} //rectification matrix
 	cv::Mat P() const {return P_;} //projection matrix
 

corelib/include/rtabmap/core/Memory.h

@@ -150,7 +150,8 @@ public:
 	SensorData getNodeData(int nodeId, bool uncompressedData = false, bool keepLoadedDataInMemory = true);
 	void getNodeWords(int nodeId,
 			std::multimap<int, cv::KeyPoint> & words,
-			std::multimap<int, cv::Point3f> & words3);
+			std::multimap<int, cv::Point3f> & words3,
+			std::multimap<int, cv::Mat> & wordsDescriptors);
 	SensorData getSignatureDataConst(int locationId) const;
 	std::set<int> getAllSignatureIds() const;
 	bool memoryChanged() const {return _memoryChanged;}

corelib/include/rtabmap/core/Optimizer.h

@@ -99,6 +99,13 @@ public:
 
 	virtual void parseParameters(const ParametersMap & parameters);
 
+	void computeBACorrespondences(
+			const std::map<int, Transform> & poses,
+			const std::multimap<int, Link> & links,
+			const std::map<int, Signature> & signatures,
+			std::map<int, cv::Point3f> & points3DMap,
+			std::map<int, std::map<int, cv::Point2f> > & wordReferences); // <ID words, IDs frames + keypoint>
+
 protected:
 	Optimizer(
 			int iterations         = Parameters::defaultOptimizerIterations(),

corelib/include/rtabmap/core/OptimizerCVSBA.h

@@ -44,29 +44,18 @@ public:
 			int iterations         = Parameters::defaultOptimizerIterations(),
 			bool slam2d            = Parameters::defaultOptimizerSlam2D(),
 			bool covarianceIgnored = Parameters::defaultOptimizerVarianceIgnored()) :
-		Optimizer(iterations, slam2d, covarianceIgnored),
+		Optimizer(iterations, slam2d, covarianceIgnored) {}
-		inlierDistance_(0.02),
-		minInliers_(10){}
 	OptimizerCVSBA(const ParametersMap & parameters) :
-		Optimizer(parameters),
+		Optimizer(parameters) {}
-		inlierDistance_(0.02),
-		minInliers_(10){}
 	virtual ~OptimizerCVSBA() {}
 
 	virtual Type type() const {return kTypeCVSBA;}
 
-	void setInlierDistance(float inlierDistance) {inlierDistance_ = inlierDistance;}
-	void setMinInliers(int minInliers) {minInliers_ = minInliers;}
-
 	virtual std::map<int, Transform> optimizeBA(
 			int rootId,
 			const std::map<int, Transform> & poses,
 			const std::multimap<int, Link> & links,
 			const std::map<int, Signature> & signatures);
-
-private:
-	float inlierDistance_;
-	float minInliers_;
 };
 
 } /* namespace rtabmap */

corelib/include/rtabmap/core/OptimizerG2O.h

@@ -68,6 +68,12 @@ public:
 			double * finalError = 0,
 			int * iterationsDone = 0);
 
+	virtual std::map<int, Transform> optimizeBA(
+			int rootId,
+			const std::map<int, Transform> & poses,
+			const std::multimap<int, Link> & links,
+			const std::map<int, Signature> & signatures);
+
 private:
 	int solver_;
 	int optimizer_;

corelib/src/Memory.cpp

@@ -2914,7 +2914,8 @@ SensorData Memory::getNodeData(int nodeId, bool uncompressedData, bool keepLoade
 
 void Memory::getNodeWords(int nodeId,
 		std::multimap<int, cv::KeyPoint> & words,
-		std::multimap<int, cv::Point3f> & words3)
+		std::multimap<int, cv::Point3f> & words3,
+		std::multimap<int, cv::Mat> & wordsDescriptors)
 {
 	UDEBUG("nodeId=%d", nodeId);
 	Signature * s = this->_getSignature(nodeId);
@@ -2922,6 +2923,7 @@ void Memory::getNodeWords(int nodeId,
 	{
 		words = s->getWords();
 		words3 = s->getWords3();
+		wordsDescriptors = s->getWordsDescriptors();
 	}
 	else if(_dbDriver)
 	{
@@ -2935,6 +2937,7 @@ void Memory::getNodeWords(int nodeId,
 		{
 			words = signatures.front()->getWords();
 			words3 = signatures.front()->getWords3();
+			wordsDescriptors = signatures.front()->getWordsDescriptors();
 			if(loadedFromTrash.size())
 			{
 				//put back

corelib/src/Optimizer.cpp

@@ -31,6 +31,8 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 #include <rtabmap/utilite/UConversion.h>
 #include <rtabmap/core/Optimizer.h>
 #include <rtabmap/core/Graph.h>
+#include <rtabmap/core/util3d_transforms.h>
+#include <rtabmap/core/RegistrationVis.h>
 #include <set>
 #include <queue>
 
@@ -269,4 +271,76 @@ void Optimizer::getConnectedGraph(
 	}
 }
 
+
+void Optimizer::computeBACorrespondences(
+		const std::map<int, Transform> & poses,
+		const std::multimap<int, Link> & links,
+		const std::map<int, Signature> & signatures,
+		std::map<int, cv::Point3f> & points3DMap,
+		std::map<int, std::map<int, cv::Point2f> > & wordReferences) // <ID words, IDs frames + keypoint>
+{
+	int wordCount = 0;
+	int edgeWithWordsAdded = 0;
+	for(std::multimap<int, Link>::const_iterator iter=links.begin(); iter!=links.end(); ++iter)
+	{
+		Link link = iter->second;
+		if(link.to() < link.from())
+		{
+			link = link.inverse();
+		}
+		if(uContains(signatures, link.from()) &&
+		   uContains(signatures, link.to()) &&
+		   uContains(poses, link.from()))
+		{
+			Signature sFrom = signatures.at(link.from());
+			Signature sTo = signatures.at(link.to());
+
+			if(sFrom.getWords().size() &&
+				sTo.getWords().size() &&
+				sFrom.getWords3().size())
+			{
+				ParametersMap regParam;
+				regParam.insert(ParametersPair(Parameters::kVisEstimationType(), "1"));
+				regParam.insert(ParametersPair(Parameters::kVisPnPReprojError(), "5"));
+				regParam.insert(ParametersPair(Parameters::kVisMinInliers(), "5"));
+				regParam.insert(ParametersPair(Parameters::kVisCorNNDR(), "0.6"));
+				RegistrationVis reg(regParam);
+
+				//sFrom.setWordsDescriptors(std::multimap<int, cv::Mat>());
+				//sTo.setWordsDescriptors(std::multimap<int, cv::Mat>());
+
+				RegistrationInfo info;
+				Transform t = reg.computeTransformationMod(sFrom, sTo, Transform(), &info);
+				//Transform t = reg.computeTransformationMod(sFrom, sTo, iter->second.transform(), &info);
+				UDEBUG("%d->%d, inliers=%d",sFrom.id(), sTo.id(), (int)info.inliersIDs.size());
+
+				if(!t.isNull())
+				{
+					Transform pose = poses.at(sFrom.id());
+					for(unsigned int i=0; i<info.inliersIDs.size(); ++i)
+					{
+						cv::Point3f p = sFrom.getWords3().lower_bound(info.inliersIDs[i])->second;
+						if(p.x > 0.0f) // make sure the point is valid
+						{
+							int wordId = ++wordCount;
+
+							p = util3d::transformPoint(p, pose);
+							points3DMap.insert(std::make_pair(wordId, p));
+							wordReferences.insert(std::make_pair(wordId, std::map<int, cv::Point2f>()));
+							wordReferences.at(wordId).insert(std::make_pair(sFrom.id(), sFrom.getWords().lower_bound(info.inliersIDs[i])->second.pt));
+							wordReferences.at(wordId).insert(std::make_pair(sTo.id(), sTo.getWords().lower_bound(info.inliersIDs[i])->second.pt));
+						}
+					}
+					++edgeWithWordsAdded;
+				}
+				else
+				{
+					UWARN("Not enough inliers (%d) between %d and %d", info.inliersIDs.size(), sFrom.id(), sTo.id());
+				}
+			}
+		}
+	}
+	UDEBUG("Added %d words (edges with words=%d/%d)", wordCount, edgeWithWordsAdded, links.size());
+}
+
 } /* namespace rtabmap */

corelib/src/OptimizerCVSBA.cpp

@@ -69,7 +69,7 @@ std::map<int, Transform> OptimizerCVSBA::optimizeBA(
 	params.iterations = this->iterations();
 	params.minError = this->epsilon();
 	params.fixedIntrinsics = 5;
-	params.fixedDistortion = 5;
+	params.fixedDistortion = 5; // updated below
 	params.verbose=ULogger::level() <= ULogger::kInfo;
 	sba.setParams(params);
 
@@ -110,7 +110,15 @@ std::map<int, Transform> OptimizerCVSBA::optimizeBA(
 			frameIdToIndex.insert(std::make_pair(iter->first, oi));
 
 			cameraMatrix[oi] = model.K();
+			if(model.D().cols != 5)
+			{
+				distCoeffs[oi] = cv::Mat::zeros(1, 5, CV_64FC1);
+				UWARN("Camera model %d: Distortion coefficients are not 5, setting all them to 0 (assuming no distortion)", iter->first);
+			}
+			else
+			{
 				distCoeffs[oi] = model.D();
+			}
 
 			Transform t = (iter->second * model.localTransform()).inverse();
 
@@ -138,88 +146,28 @@ std::map<int, Transform> OptimizerCVSBA::optimizeBA(
 	distCoeffs.resize(oi);
 
 	std::map<int, cv::Point3f> points3DMap;
-	std::multimap<int, std::pair<int, cv::Point2f> > wordReferences; // <ID words, IDs frames + keypoint>
+	std::map<int, std::map<int, cv::Point2f> > wordReferences; // <ID words, IDs frames + keypoint>
-	for(std::multimap<int, Link>::const_iterator iter=links.begin(); iter!=links.end(); ++iter)
+	computeBACorrespondences(frames, links, signatures, points3DMap, wordReferences);
-	{
-		Link link = iter->second;
-		if(link.to() < link.from())
-		{
-			link = link.inverse();
-		}
-		if(uContains(signatures, link.from()) &&
-		   uContains(signatures, link.to()) &&
-		   uContains(frames, link.from()))
-		{
-			const Signature & sFrom = signatures.at(link.from());
-			const Signature & sTo = signatures.at(link.to());
 
-			std::vector<int> inliers;
+	UDEBUG("points=%d frames=%d", (int)wordReferences.size(), (int)frames.size());
-			Transform t = util3d::estimateMotion3DTo3D(
+	std::vector<cv::Point3f> points(wordReferences.size()); //npoints
-					uMultimapToMapUnique(sFrom.getWords3()),
-					uMultimapToMapUnique(sTo.getWords3()),
-					minInliers_,
-					inlierDistance_,
-					100,
-					10,
-					0,
-					0,
-					&inliers);
-
-			if(!t.isNull())
-			{
-				Transform pose = frames.at(sFrom.id());
-				for(unsigned int i=0; i<inliers.size(); ++i)
-				{
-					cv::Point3f p = util3d::transformPoint(sFrom.getWords3().lower_bound(inliers[i])->second, pose);
-					std::map<int, cv::Point3f>::iterator jter = points3DMap.find(inliers[i]);
-					if(jter == points3DMap.end())
-					{
-						points3DMap.insert(std::make_pair(inliers[i], p));
-						wordReferences.insert(std::make_pair(inliers[i], std::make_pair(sFrom.id(), sFrom.getWords().lower_bound(inliers[i])->second.pt)));
-						wordReferences.insert(std::make_pair(inliers[i], std::make_pair(sTo.id(), sTo.getWords().lower_bound(inliers[i])->second.pt)));
-					}
-					else
-					{
-						float dist = uNorm(p.x - jter->second.x, p.y - jter->second.y, p.z - jter->second.z);
-						if(dist <= inlierDistance_)
-						{
-							// in case of loop closure links
-							wordReferences.insert(std::make_pair(inliers[i], std::make_pair(sFrom.id(), sFrom.getWords().lower_bound(inliers[i])->second.pt)));
-							wordReferences.insert(std::make_pair(inliers[i], std::make_pair(sTo.id(), sTo.getWords().lower_bound(inliers[i])->second.pt)));
-						}
-					}
-				}
-			}
-			else
-			{
-				UWARN("Not enough inliers (%d) between %d and %d", inliers.size(), sFrom.id(), sTo.id());
-			}
-		}
-	}
-
-	std::list<int> wordReferencesKeys = uUniqueKeys(wordReferences);
-	UDEBUG("points=%d frames=%d", (int)wordReferencesKeys.size(), (int)frames.size());
-	std::vector<cv::Point3f> points(wordReferencesKeys.size()); //npoints
 	std::vector<std::vector<cv::Point2f> >  imagePoints(frames.size()); //nframes -> npoints
 	std::vector<std::vector<int> > visibility(frames.size()); //nframes -> npoints
 	for(unsigned int i=0; i<frames.size(); ++i)
 	{
-		imagePoints[i].resize(wordReferencesKeys.size(), cv::Point2f(std::numeric_limits<float>::quiet_NaN(), std::numeric_limits<float>::quiet_NaN()));
+		imagePoints[i].resize(wordReferences.size(), cv::Point2f(std::numeric_limits<float>::quiet_NaN(), std::numeric_limits<float>::quiet_NaN()));
-		visibility[i].resize(wordReferencesKeys.size(), 0);
+		visibility[i].resize(wordReferences.size(), 0);
 	}
 	int i=0;
-	for(std::list<int>::iterator iter = wordReferencesKeys.begin(); iter!=wordReferencesKeys.end(); ++iter)
+	for(std::map<int, std::map<int, cv::Point2f> >::iterator iter = wordReferences.begin(); iter!=wordReferences.end(); ++iter)
 	{
-		points[i] = points3DMap.at(*iter);
+		points[i] = points3DMap.at(iter->first);
 
-		std::multimap<int, std::pair<int, cv::Point2f> >::iterator jter = wordReferences.lower_bound(*iter);
+		for(std::map<int, cv::Point2f>::const_iterator jter=iter->second.begin(); jter!=iter->second.end(); ++jter)
-		while(jter->first == *iter && jter != wordReferences.end())
 		{
-			imagePoints[frameIdToIndex.at(jter->second.first)][i] = jter->second.second;
+			imagePoints[frameIdToIndex.at(jter->first)][i] = jter->second;
-			visibility[frameIdToIndex.at(jter->second.first)][i] = 1;
+			visibility[frameIdToIndex.at(jter->first)][i] = 1;
-			++jter;
 		}
-
 		++i;
 	}
 

corelib/src/OptimizerG2O.cpp

@@ -33,6 +33,8 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 #include <set>
 
 #include <rtabmap/core/OptimizerG2O.h>
+#include <rtabmap/core/util3d_transforms.h>
+#include <rtabmap/core/util3d_motion_estimation.h>
 
 #ifdef RTABMAP_G2O
 #include "g2o/config.h"
@@ -42,6 +44,9 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 #include "g2o/core/optimization_algorithm_factory.h"
 #include "g2o/core/optimization_algorithm_gauss_newton.h"
 #include "g2o/core/optimization_algorithm_levenberg.h"
+#include "g2o/core/linear_solver.h"
+#include "g2o/types/sba/types_sba.h"
+#include "g2o/core/robust_kernel_impl.h"
 #ifdef G2O_HAVE_CSPARSE
 #include "g2o/solvers/csparse/linear_solver_csparse.h"
 #endif
@@ -152,12 +157,10 @@ std::map<int, Transform> OptimizerG2O::optimize(
 
 		g2o::SparseOptimizer optimizer;
 		optimizer.setVerbose(ULogger::level()==ULogger::kDebug);
-		int solverApproach = 0;
-		int optimizationApproach = 1;
 
 		SlamBlockSolver * blockSolver = 0;
 
-		if(solverApproach == 2)
+		if(solver_ == 2)
 		{
 #ifdef G2O_HAVE_CHOLMOD
 			//chmold
@@ -166,7 +169,7 @@ std::map<int, Transform> OptimizerG2O::optimize(
 			blockSolver = new SlamBlockSolver(linearSolver);
 #endif
 		}
-		else if(solverApproach == 0)
+		else if(solver_ == 0)
 		{
 #ifdef G2O_HAVE_CSPARSE
 			//csparse
@@ -183,7 +186,7 @@ std::map<int, Transform> OptimizerG2O::optimize(
 			blockSolver = new SlamBlockSolver(linearSolver);
 		}
 
-		if(optimizationApproach == 1)
+		if(optimizer_ == 1)
 		{
 			optimizer.setAlgorithm(new g2o::OptimizationAlgorithmGaussNewton(blockSolver));
 		}
@@ -538,6 +541,307 @@ std::map<int, Transform> OptimizerG2O::optimize(
 	return optimizedPoses;
 }
 
+std::map<int, Transform> OptimizerG2O::optimizeBA(
+		int rootId,
+		const std::map<int, Transform> & poses,
+		const std::multimap<int, Link> & links,
+		const std::map<int, Signature> & signatures)
+{
+	std::map<int, Transform> optimizedPoses;
+#ifdef RTABMAP_G2O
+	UDEBUG("Optimizing graph...");
+
+	optimizedPoses.clear();
+	if(links.size()>=1 && poses.size()>=2 && iterations() > 0)
+	{
+		g2o::SparseOptimizer optimizer;
+		optimizer.setVerbose(ULogger::level()==ULogger::kDebug);
+		g2o::BlockSolver_6_3::LinearSolverType * linearSolver = 0;
+		bool robustKernel = true;
+
+		if(solver_ == 2)
+		{
+#ifdef G2O_HAVE_CHOLMOD
+			//chmold
+			linearSolver = new g2o::LinearSolverCholmod<g2o::BlockSolver_6_3::PoseMatrixType>();
+#endif
+		}
+		else if(solver_ == 0)
+		{
+#ifdef G2O_HAVE_CSPARSE
+			//csparse
+			linearSolver = new g2o::LinearSolverCSparse<g2o::BlockSolver_6_3::PoseMatrixType>();
+#endif
+		}
+
+		if(linearSolver == 0)
+		{
+			//pcg
+			linearSolver = new g2o::LinearSolverPCG<g2o::BlockSolver_6_3::PoseMatrixType>();
+		}
+
+		g2o::BlockSolver_6_3 * solver_ptr = new g2o::BlockSolver_6_3(linearSolver);
+
+		if(optimizer_ == 1)
+		{
+			optimizer.setAlgorithm(new g2o::OptimizationAlgorithmGaussNewton(solver_ptr));
+		}
+		else
+		{
+			optimizer.setAlgorithm(new g2o::OptimizationAlgorithmLevenberg(solver_ptr));
+		}
+
+		std::map<int, Transform> frames = poses;
+
+		UDEBUG("fill poses to g2o...");
+		std::map<int, CameraModel> models;
+		for(std::map<int, Transform>::iterator iter=frames.begin(); iter!=frames.end(); )
+		{
+			// Get camera model
+			CameraModel model;
+			if(uContains(signatures, iter->first))
+			{
+				if(signatures.at(iter->first).sensorData().cameraModels().size() == 1 && signatures.at(iter->first).sensorData().cameraModels().at(0).isValidForProjection())
+				{
+					model = signatures.at(iter->first).sensorData().cameraModels()[0];
+				}
+				else if(signatures.at(iter->first).sensorData().stereoCameraModel().isValidForProjection())
+				{
+					model = signatures.at(iter->first).sensorData().stereoCameraModel().left();
+				}
+				else
+				{
+					UERROR("Missing calibration for node %d", iter->first);
+				}
+			}
+			else
+			{
+				UERROR("Did not find node %d in cache", iter->first);
+			}
+
+			if(model.isValidForProjection())
+			{
+				models.insert(std::make_pair(iter->first, model));
+				Transform camPose = iter->second * model.localTransform();
+				//iter->second = (iter->second * model.localTransform()).inverse();
+				UDEBUG("%d t=%s", iter->first, camPose.prettyPrint().c_str());
+
+				// Add node's pose
+				UASSERT(!camPose.isNull());
+				g2o::VertexCam * vCam = new g2o::VertexCam();
+
+				Eigen::Affine3d a = camPose.toEigen3d();
+				g2o::SBACam cam(Eigen::Quaterniond(a.rotation()), a.translation());
+				cam.setKcam(model.fx(), model.fy(), model.cx(), model.cy(), 0);
+				vCam->setEstimate(cam);
+				if(iter->first == rootId)
+				{
+					vCam->setFixed(true);
+				}
+				vCam->setId(iter->first);
+				std::cout << cam << std::endl;
+				UASSERT_MSG(optimizer.addVertex(vCam), uFormat("cannot insert vertex %d!?", iter->first).c_str());
+
+				++iter;
+			}
+			else
+			{
+				frames.erase(iter++);
+			}
+		}
+
+		UDEBUG("fill edges to g2o and associate each 3D point to all frames observing it...");
+		for(std::multimap<int, Link>::const_iterator iter=links.begin(); iter!=links.end(); ++iter)
+		{
+			Link link = iter->second;
+			if(link.to() < link.from())
+			{
+				link = link.inverse();
+			}
+			if(uContains(signatures, link.from()) &&
+			   uContains(signatures, link.to()) &&
+			   uContains(frames, link.from()) &&
+			   uContains(frames, link.to()))
+			{
+				// add edge
+				int id1 = iter->first;
+				int id2 = iter->second.to();
+
+				UASSERT(!iter->second.transform().isNull());
+
+				Eigen::Matrix<double, 6, 6> information = Eigen::Matrix<double, 6, 6>::Identity();
+				if(!isCovarianceIgnored())
+				{
+					memcpy(information.data(), iter->second.infMatrix().data, iter->second.infMatrix().total()*sizeof(double));
+				}
+
+				// between cameras, not base_link
+				Transform camLink = models.at(id1).localTransform().inverse()*iter->second.transform()*models.at(id2).localTransform();
+				//Transform t = iter->second.transform();
+				UDEBUG("added edge %d=%s -> %d=%s",
+						id1,
+						iter->second.transform().prettyPrint().c_str(),
+						id2,
+						camLink.prettyPrint().c_str());
+				Eigen::Affine3d a = camLink.toEigen3d();
+
+				g2o::EdgeSBACam * e = new g2o::EdgeSBACam();
+				g2o::VertexSE3* v1 = (g2o::VertexSE3*)optimizer.vertex(id1);
+				g2o::VertexSE3* v2 = (g2o::VertexSE3*)optimizer.vertex(id2);
+				UASSERT(v1 != 0);
+				UASSERT(v2 != 0);
+				e->setVertex(0, v1);
+				e->setVertex(1, v2);
+				e->setMeasurement(g2o::SE3Quat(a.rotation(), a.translation()));
+				e->setInformation(information);
+
+				if (!optimizer.addEdge(e))
+				{
+					delete e;
+					UERROR("Map: Failed adding constraint between %d and %d, skipping", id1, id2);
+				}
+			}
+		}
+
+		std::map<int, cv::Point3f> points3DMap;
+		std::map<int, std::map<int, cv::Point2f> > wordReferences; // <ID words, IDs frames + keypoint>
+		this->computeBACorrespondences(frames, links, signatures, points3DMap, wordReferences);
+
+		UDEBUG("fill 3D points to g2o...");
+		int stepVertexId = frames.rbegin()->first+1;
+		for(std::map<int, std::map<int, cv::Point2f> >::iterator iter = wordReferences.begin(); iter!=wordReferences.end(); ++iter)
+		{
+			const cv::Point3f & pt3d = points3DMap.at(iter->first);
+			g2o::VertexSBAPointXYZ* vpt3d = new g2o::VertexSBAPointXYZ();
+
+			vpt3d->setEstimate(Eigen::Vector3d(pt3d.x, pt3d.y, pt3d.z));
+			vpt3d->setId(stepVertexId + iter->first);
+			vpt3d->setMarginalized(true);
+			optimizer.addVertex(vpt3d);
+
+			// set observations
+			for(std::map<int, cv::Point2f>::const_iterator jter=iter->second.begin(); jter!=iter->second.end(); ++jter)
+			{
+				int camId = jter->first;
+
+				const cv::Point2f & pt = jter->second;
+
+				Eigen::Matrix<double,2,1> obs;
+				obs << pt.x, pt.y;
+
+				UDEBUG("Added observation pt=%d to cam=%d (%f,%f)", vpt3d->id(), camId, pt.x, pt.y);
+
+				g2o::EdgeProjectP2MC* e = new g2o::EdgeProjectP2MC();
+
+				e->setVertex(0, vpt3d);
+				e->setVertex(1, dynamic_cast<g2o::OptimizableGraph::Vertex*>(optimizer.vertex(camId)));
+				e->setMeasurement(obs);
+				e->setInformation(Eigen::Matrix2d::Identity());
+
+				if(robustKernel)
+				{
+					e->setRobustKernel(new g2o::RobustKernelHuber);
+				}
+
+				optimizer.addEdge(e);
+			}
+		}
+
+		UDEBUG("Initial optimization...");
+		optimizer.initializeOptimization();
+
+		UASSERT(optimizer.verifyInformationMatrices());
+
+		UINFO("g2o optimizing begin (max iterations=%d, robustKernel=%d)", iterations(), robustKernel?1:0);
+
+		int it = 0;
+		UTimer timer;
+		double lastError = 0.0;
+		if(this->epsilon() > 0.0)
+		{
+			for(int i=0; i<iterations(); ++i)
+			{
+				it += optimizer.optimize(1);
+
+				// early stop condition
+				optimizer.computeActiveErrors();
+				double chi2 = optimizer.activeRobustChi2();
+				UDEBUG("iteration %d: %d nodes, %d edges, chi2: %f", i, (int)optimizer.vertices().size(), (int)optimizer.edges().size(), chi2);
+
+				if(i>0 && (optimizer.activeRobustChi2() > 1000000000000.0 || !uIsFinite(optimizer.activeRobustChi2())))
+				{
+					UWARN("g2o: Large optimimzation error detected (%f), aborting optimization!");
+					return optimizedPoses;
+				}
+
+				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
+		{
+			it = optimizer.optimize(iterations());
+			optimizer.computeActiveErrors();
+			UDEBUG("%d nodes, %d edges, chi2: %f", (int)optimizer.vertices().size(), (int)optimizer.edges().size(), optimizer.activeRobustChi2());
+		}
+		UINFO("g2o optimizing end (%d iterations done, error=%f, time = %f s)", it, optimizer.activeRobustChi2(), timer.ticks());
+
+		if(optimizer.activeRobustChi2() > 1000000000000.0)
+		{
+			UWARN("g2o: Large optimimzation error detected (%f), aborting optimization!");
+			return optimizedPoses;
+		}
+
+		for(std::map<int, Transform>::const_iterator iter = poses.begin(); iter!=poses.end(); ++iter)
+		{
+			const g2o::VertexCam* v = (const g2o::VertexCam*)optimizer.vertex(iter->first);
+			if(v)
+			{
+				Transform t = Transform::fromEigen3d(v->estimate());
+				UDEBUG("%d t=%s", iter->first, t.prettyPrint().c_str());
+				// remove model local transform
+				t *= models.at(iter->first).localTransform().inverse();
+				optimizedPoses.insert(std::pair<int, Transform>(iter->first, t));
+				UASSERT_MSG(!t.isNull(), uFormat("Optimized pose %d is null!?!?", iter->first).c_str());
+			}
+			else
+			{
+				UERROR("Vertex %d not found!?", iter->first);
+			}
+		}
+	}
+	else if(poses.size() == 1 || iterations() <= 0)
+	{
+		optimizedPoses = poses;
+	}
+	else
+	{
+		UWARN("This method should be called at least with 1 pose!");
+	}
+	UDEBUG("Optimizing graph...end!");
+#else
+	UERROR("Not built with G2O support!");
+#endif
+	return optimizedPoses;
+}
+
 bool OptimizerG2O::saveGraph(
 		const std::string & fileName,
 		const std::map<int, Transform> & poses,

corelib/src/Rtabmap.cpp

@@ -3160,7 +3160,8 @@ void Rtabmap::get3DMap(
 			data.setId(*iter);
 			std::multimap<int, cv::KeyPoint> words;
 			std::multimap<int, cv::Point3f> words3;
-			_memory->getNodeWords(*iter, words, words3);
+			std::multimap<int, cv::Mat> wordsDescriptors;
+			_memory->getNodeWords(*iter, words, words3, wordsDescriptors);
 			signatures.insert(std::make_pair(*iter,
 					Signature(*iter,
 							mapId,
@@ -3172,6 +3173,7 @@ void Rtabmap::get3DMap(
 							data)));
 			signatures.at(*iter).setWords(words);
 			signatures.at(*iter).setWords3(words3);
+			signatures.at(*iter).setWordsDescriptors(wordsDescriptors);
 		}
 	}
 	else if(_memory && (_memory->getStMem().size() || _memory->getWorkingMem().size() > 1))

corelib/src/util3d_features.cpp

@@ -100,7 +100,7 @@ std::vector<cv::Point3f> generateKeypoints3DDepth(
 
 			cv::Point3f pt(bad_point, bad_point, bad_point);
 			if(pcl::isFinite(ptXYZ) &&
-				(minDepth <= 0.0f || ptXYZ.z >= minDepth) &&
+				(minDepth < 0.0f || ptXYZ.z > minDepth) &&
 				(maxDepth <= 0.0f || ptXYZ.z <= maxDepth))
 			{
 				pt = cv::Point3f(ptXYZ.x, ptXYZ.y, ptXYZ.z);
@@ -137,7 +137,7 @@ std::vector<cv::Point3f> generateKeypoints3DDisparity(
 
 		cv::Point3f pt(bad_point, bad_point, bad_point);
 		if(util3d::isFinite(tmpPt) &&
-			(minDepth <= 0.0f || tmpPt.z >= minDepth) &&
+			(minDepth < 0.0f || tmpPt.z > minDepth) &&
 			(maxDepth <= 0.0f || tmpPt.z <= maxDepth))
 		{
 			pt = tmpPt;
@@ -181,7 +181,7 @@ std::vector<cv::Point3f> generateKeypoints3DStereo(
 						model);
 
 				if(util3d::isFinite(tmpPt) &&
-				   (minDepth <= 0.0f || tmpPt.z >= minDepth) &&
+				   (minDepth < 0.0f || tmpPt.z > minDepth) &&
 				   (maxDepth <= 0.0f || tmpPt.z <= maxDepth))
 				{
 					pt = tmpPt;
@@ -221,7 +221,7 @@ std::map<int, cv::Point3f> generateWords3DMono(
 	std::map<int, cv::Point3f> words3D;
 	std::list<std::pair<int, std::pair<cv::KeyPoint, cv::KeyPoint> > > pairs;
 	int pairsFound = EpipolarGeometry::findPairs(refWords, nextWords, pairs);
-	UDEBUG("pairsFound=%d", pairsFound);
+	UDEBUG("pairsFound=%d/%d", pairsFound, int(refWords.size()>nextWords.size()?refWords.size():nextWords.size()));
 	if(pairsFound > 8)
 	{
 		std::vector<unsigned char> status;

guilib/src/MainWindow.cpp

@@ -3879,7 +3879,6 @@ void MainWindow::postProcessing()
 	}
 
 	bool detectMoreLoopClosures = _postProcessingDialog->isDetectMoreLoopClosures();
-	bool reextractFeatures = _postProcessingDialog->isReextractFeatures();
 	bool refineNeighborLinks = _postProcessingDialog->isRefineNeighborLinks();
 	bool refineLoopClosureLinks = _postProcessingDialog->isRefineLoopClosureLinks();
 	double clusterRadius = _postProcessingDialog->clusterRadius();
@@ -3888,8 +3887,7 @@ void MainWindow::postProcessing()
 	bool sba = _postProcessingDialog->isSBA();
 	int sbaIterations = _postProcessingDialog->sbaIterations();
 	double sbaEpsilon = _postProcessingDialog->sbaEpsilon();
-	double sbaInlierDistance = _postProcessingDialog->sbaInlierDistance();
+	Optimizer::Type sbaType = _postProcessingDialog->sbaType();
-	int sbaMinInliers = _postProcessingDialog->sbaMinInliers();
 
 	if(!detectMoreLoopClosures && !refineNeighborLinks && !refineLoopClosureLinks && !sba)
 	{
@@ -3960,6 +3958,7 @@ void MainWindow::postProcessing()
 
 	bool warn = false;
 	int loopClosuresAdded = 0;
+	std::multimap<int, int> checkedLoopClosures;
 	if(detectMoreLoopClosures)
 	{
 		UDEBUG("");
@@ -3990,10 +3989,25 @@ void MainWindow::postProcessing()
 					to = iter->first;
 				}
 
+				bool alreadyChecked = false;
+				for(std::multimap<int, int>::iterator jter = checkedLoopClosures.lower_bound(from);
+					!alreadyChecked && jter!=checkedLoopClosures.end() && jter->first == from;
+					++jter)
+				{
+					if(to == jter->second)
+					{
+						alreadyChecked = true;
+					}
+				}
+
+				if(!alreadyChecked)
+				{
 					// only add new links and one per cluster per iteration
-				if(addedLinks.find(from) == addedLinks.end() && addedLinks.find(to) == addedLinks.end() &&
+					if(addedLinks.find(from) == addedLinks.end() &&
 					   rtabmap::graph::findLink(_currentLinksMap, from, to) == _currentLinksMap.end())
 					{
+						checkedLoopClosures.insert(std::make_pair(from, to));
+
 						if(!_cachedSignatures.contains(from))
 						{
 							UERROR("Didn't find signature %d", from);
@@ -4013,31 +4027,6 @@ void MainWindow::postProcessing()
 							if(signatureFrom.getWeight() >= 0 &&
 							   signatureTo.getWeight() >= 0) // ignore intermediate nodes
 							{
-							if(reextractFeatures)
-							{
-								signatureFrom.setWords(std::multimap<int, cv::KeyPoint>());
-								signatureFrom.setWords3(std::multimap<int, cv::Point3f>());
-								signatureTo.setWords(std::multimap<int, cv::KeyPoint>());
-								signatureTo.setWords3(std::multimap<int, cv::Point3f>());
-
-								if(signatureFrom.sensorData().imageRaw().empty())
-								{
-									signatureFrom.sensorData().uncompressData();
-									if(signatureFrom.sensorData().imageRaw().empty())
-									{
-										continue;
-									}
-								}
-								if(signatureTo.sensorData().imageRaw().empty())
-								{
-									signatureTo.sensorData().uncompressData();
-									if(signatureTo.sensorData().imageRaw().empty())
-									{
-										continue;
-									}
-								}
-							}
-
 								Transform transform;
 								RegistrationInfo info;
 								RegistrationVis registration(parameters);
@@ -4056,6 +4045,7 @@ void MainWindow::postProcessing()
 						}
 					}
 				}
+			}
 			_initProgressDialog->appendText(tr("Iteration %1/%2: Detected %3 loop closures!")
 					.arg(n+1).arg(detectLoopClosureIterations).arg(addedLinks.size()/2));
 			if(addedLinks.size() == 0)
@@ -4186,7 +4176,7 @@ void MainWindow::postProcessing()
 
 	if(sba)
 	{
-		UASSERT(OptimizerCVSBA::available());
+		UASSERT(Optimizer::isAvailable(sbaType));
 		_initProgressDialog->appendText(tr("SBA (%1 nodes, %2 constraints, %3 iterations)...")
 					.arg(optimizedPoses.size()).arg(linksOut.size()).arg(sbaIterations));
 		QApplication::processEvents();
@@ -4196,10 +4186,9 @@ void MainWindow::postProcessing()
 		ParametersMap parametersSBA = _preferencesDialog->getAllParameters();
 		uInsert(parametersSBA, std::make_pair(Parameters::kOptimizerIterations(), uNumber2Str(sbaIterations)));
 		uInsert(parametersSBA, std::make_pair(Parameters::kOptimizerEpsilon(), uNumber2Str(sbaEpsilon)));
-		OptimizerCVSBA cvsba = OptimizerCVSBA(parametersSBA);
+		Optimizer * sba = Optimizer::create(sbaType, parametersSBA);
-		cvsba.setInlierDistance(sbaInlierDistance);
+		std::map<int, Transform>  newPoses = sba->optimizeBA(optimizedPoses.begin()->first, optimizedPoses, linksOut, _cachedSignatures.toStdMap());
-		cvsba.setMinInliers(sbaMinInliers);
+		delete sba;
-		std::map<int, Transform>  newPoses = cvsba.optimizeBA(0, optimizedPoses, linksOut, _cachedSignatures.toStdMap());
 		if(newPoses.size())
 		{
 			optimizedPoses = newPoses;

guilib/src/PostProcessingDialog.cpp

@@ -39,11 +39,21 @@ PostProcessingDialog::PostProcessingDialog(QWidget * parent) :
 	_ui = new Ui_PostProcessingDialog();
 	_ui->setupUi(this);
 
-	if(!Optimizer::isAvailable(Optimizer::kTypeCVSBA))
+	if(!Optimizer::isAvailable(Optimizer::kTypeCVSBA) && !Optimizer::isAvailable(Optimizer::kTypeG2O))
 	{
 		_ui->sba->setEnabled(false);
 		_ui->sba->setChecked(false);
 	}
+	else if(!Optimizer::isAvailable(Optimizer::kTypeCVSBA))
+	{
+		_ui->comboBox_sbaType->setItemData(1, 0, Qt::UserRole - 1);
+		_ui->comboBox_sbaType->setCurrentIndex(0);
+	}
+	else if(!Optimizer::isAvailable(Optimizer::kTypeG2O))
+	{
+		_ui->comboBox_sbaType->setItemData(0, 0, Qt::UserRole - 1);
+		_ui->comboBox_sbaType->setCurrentIndex(1);
+	}
 
 	restoreDefaults();
 
@@ -57,15 +67,13 @@ PostProcessingDialog::PostProcessingDialog(QWidget * parent) :
 	connect(_ui->clusterRadius, SIGNAL(valueChanged(double)), this, SIGNAL(configChanged()));
 	connect(_ui->clusterAngle, SIGNAL(valueChanged(double)), this, SIGNAL(configChanged()));
 	connect(_ui->iterations, SIGNAL(valueChanged(int)), this, SIGNAL(configChanged()));
-	connect(_ui->reextractFeatures, SIGNAL(stateChanged(int)), this, SIGNAL(configChanged()));
 	connect(_ui->refineNeighborLinks, SIGNAL(stateChanged(int)), this, SIGNAL(configChanged()));
 	connect(_ui->refineLoopClosureLinks, SIGNAL(stateChanged(int)), this, SIGNAL(configChanged()));
 
 	connect(_ui->sba, SIGNAL(clicked(bool)), this, SIGNAL(configChanged()));
 	connect(_ui->sba_iterations, SIGNAL(valueChanged(int)), this, SIGNAL(configChanged()));
 	connect(_ui->sba_epsilon, SIGNAL(valueChanged(double)), this, SIGNAL(configChanged()));
-	connect(_ui->sba_minInlierDistance, SIGNAL(valueChanged(double)), this, SIGNAL(configChanged()));
+	connect(_ui->comboBox_sbaType, SIGNAL(indexChanged(int)), this, SIGNAL(configChanged()));
-	connect(_ui->sba_minInliers, SIGNAL(valueChanged(int)), this, SIGNAL(configChanged()));
 }
 
 PostProcessingDialog::~PostProcessingDialog()
@@ -83,14 +91,12 @@ void PostProcessingDialog::saveSettings(QSettings & settings, const QString & gr
 	settings.setValue("cluster_radius", this->clusterRadius());
 	settings.setValue("cluster_angle", this->clusterAngle());
 	settings.setValue("iterations", this->iterations());
-	settings.setValue("reextract_features", this->isReextractFeatures());
 	settings.setValue("refine_neigbors", this->isRefineNeighborLinks());
 	settings.setValue("refine_lc", this->isRefineLoopClosureLinks());
 	settings.setValue("sba", this->isSBA());
 	settings.setValue("sba_iterations", this->sbaIterations());
 	settings.setValue("sba_epsilon", this->sbaEpsilon());
-	settings.setValue("sba_inlier_distance", this->sbaInlierDistance());
+	settings.setValue("sba_type", this->sbaType());
-	settings.setValue("sba_min_inliers", this->sbaMinInliers());
 	if(!group.isEmpty())
 	{
 		settings.endGroup();
@@ -107,14 +113,12 @@ void PostProcessingDialog::loadSettings(QSettings & settings, const QString & gr
 	this->setClusterRadius(settings.value("cluster_radius", this->clusterRadius()).toDouble());
 	this->setClusterAngle(settings.value("cluster_angle", this->clusterAngle()).toDouble());
 	this->setIterations(settings.value("iterations", this->iterations()).toInt());
-	this->setReextractFeatures(settings.value("reextract_features", this->isReextractFeatures()).toBool());
 	this->setRefineNeighborLinks(settings.value("refine_neigbors", this->isRefineNeighborLinks()).toBool());
 	this->setRefineLoopClosureLinks(settings.value("refine_lc", this->isRefineLoopClosureLinks()).toBool());
 	this->setSBA(settings.value("sba", this->isSBA()).toBool());
 	this->setSBAIterations(settings.value("sba_iterations", this->sbaIterations()).toInt());
 	this->setSBAEpsilon(settings.value("sba_epsilon", this->sbaEpsilon()).toDouble());
-	this->setSBAInlierDistance(settings.value("sba_inlier_distance", this->sbaInlierDistance()).toDouble());
+	this->setSBAType((Optimizer::Type)settings.value("sba_type", this->sbaType()).toInt());
-	this->setSBAMinInliers(settings.value("sba_min_inliers", this->sbaMinInliers()).toInt());
 	if(!group.isEmpty())
 	{
 		settings.endGroup();
@@ -127,14 +131,12 @@ void PostProcessingDialog::restoreDefaults()
 	setClusterRadius(0.5);
 	setClusterAngle(30);
 	setIterations(1);
-	setReextractFeatures(false);
 	setRefineNeighborLinks(false);
 	setRefineLoopClosureLinks(false);
 	setSBA(false);
 	setSBAIterations(20);
 	setSBAEpsilon(0.0001);
-	setSBAInlierDistance(0.05);
+	setSBAType(!Optimizer::isAvailable(Optimizer::kTypeG2O)&&Optimizer::isAvailable(Optimizer::kTypeCVSBA)?Optimizer::kTypeCVSBA:Optimizer::kTypeG2O);
-	setSBAMinInliers(10);
 }
 
 void PostProcessingDialog::updateButtonBox()
@@ -163,11 +165,6 @@ int PostProcessingDialog::iterations() const
 	return _ui->iterations->value();
 }
 
-bool PostProcessingDialog::isReextractFeatures() const
-{
-	return _ui->reextractFeatures->isChecked();
-}
-
 bool PostProcessingDialog::isRefineNeighborLinks() const
 {
 	return _ui->refineNeighborLinks->isChecked();
@@ -191,13 +188,9 @@ double PostProcessingDialog::sbaEpsilon() const
 {
 	return _ui->sba_epsilon->value();
 }
-double PostProcessingDialog::sbaInlierDistance() const
+Optimizer::Type PostProcessingDialog::sbaType() const
 {
-	return _ui->sba_minInlierDistance->value();
+	return _ui->comboBox_sbaType->currentIndex()==0?Optimizer::kTypeG2O:Optimizer::kTypeCVSBA;
-}
-int PostProcessingDialog::sbaMinInliers() const
-{
-	return _ui->sba_minInliers->value();
 }
 
 //setters
@@ -217,10 +210,6 @@ void PostProcessingDialog::setIterations(int iterations)
 {
 	_ui->iterations->setValue(iterations);
 }
-void PostProcessingDialog::setReextractFeatures(bool on)
-{
-	_ui->reextractFeatures->setChecked(on);
-}
 void PostProcessingDialog::setRefineNeighborLinks(bool on)
 {
 	_ui->refineNeighborLinks->setChecked(on);
@@ -241,13 +230,16 @@ void PostProcessingDialog::setSBAEpsilon(double epsilon)
 {
 	_ui->sba_epsilon->setValue(epsilon);
 }
-void PostProcessingDialog::setSBAInlierDistance(double inlierDistance)
+void PostProcessingDialog::setSBAType(Optimizer::Type type)
 {
-	_ui->sba_minInlierDistance->setValue(inlierDistance);
+	if(type == Optimizer::kTypeCVSBA)
+	{
+		_ui->comboBox_sbaType->setCurrentIndex(1);
 	}
-void PostProcessingDialog::setSBAMinInliers(int minInliers)
+	else
 	{
-	_ui->sba_minInliers->setValue(minInliers);
+		_ui->comboBox_sbaType->setCurrentIndex(0);
+	}
 }
 
 

guilib/src/PostProcessingDialog.h

@@ -31,6 +31,8 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 #include <QDialog>
 #include <QtCore/QSettings>
 
+#include <rtabmap/core/Optimizer.h>
+
 class Ui_PostProcessingDialog;
 class QAbstractButton;
 
@@ -53,28 +55,24 @@ public:
 	double clusterRadius() const;
 	double clusterAngle() const;
 	int iterations() const;
-	bool isReextractFeatures() const;
 	bool isRefineNeighborLinks() const;
 	bool isRefineLoopClosureLinks() const;
 	bool isSBA() const;
 	int sbaIterations() const;
 	double sbaEpsilon() const;
-	double sbaInlierDistance() const;
+	Optimizer::Type sbaType() const;
-	int sbaMinInliers() const;
 
 	//setters
 	void setDetectMoreLoopClosures(bool on);
 	void setClusterRadius(double radius);
 	void setClusterAngle(double angle);
 	void setIterations(int iterations);
-	void setReextractFeatures(bool on);
 	void setRefineNeighborLinks(bool on);
 	void setRefineLoopClosureLinks(bool on);
 	void setSBA(bool on);
 	void setSBAIterations(int iterations);
 	void setSBAEpsilon(double epsilon);
-	void setSBAInlierDistance(double inlierDistance);
+	void setSBAType(Optimizer::Type type);
-	void setSBAMinInliers(int minInliers);
 
 signals:
 	void configChanged();

guilib/src/ui/postProcessingDialog.ui

@@ -87,26 +87,6 @@
           </property>
          </widget>
         </item>
-        <item row="3" column="1">
-         <widget class="QLabel" name="label_5">
-          <property name="text">
-           <string>Re-extract features</string>
-          </property>
-          <property name="wordWrap">
-           <bool>true</bool>
-          </property>
-         </widget>
-        </item>
-        <item row="3" column="0">
-         <widget class="QCheckBox" name="reextractFeatures">
-          <property name="text">
-           <string/>
-          </property>
-          <property name="checked">
-           <bool>false</bool>
-          </property>
-         </widget>
-        </item>
         <item row="2" column="1">
          <widget class="QLabel" name="label_6">
           <property name="text">
@@ -201,72 +181,7 @@
      <layout class="QVBoxLayout" name="verticalLayout_4">
       <item>
        <layout class="QGridLayout" name="gridLayout_2" columnstretch="0,1">
-        <item row="0" column="1">
+        <item row="1" column="0">
-         <widget class="QLabel" name="label_8">
-          <property name="text">
-           <string>Iterations</string>
-          </property>
-          <property name="wordWrap">
-           <bool>true</bool>
-          </property>
-         </widget>
-        </item>
-        <item row="2" column="1">
-         <widget class="QLabel" name="label_10">
-          <property name="text">
-           <string>Inlier distance of corresponding 3D words</string>
-          </property>
-          <property name="wordWrap">
-           <bool>true</bool>
-          </property>
-         </widget>
-        </item>
-        <item row="2" column="0">
-         <widget class="QDoubleSpinBox" name="sba_minInlierDistance">
-          <property name="suffix">
-           <string> m</string>
-          </property>
-          <property name="decimals">
-           <number>3</number>
-          </property>
-          <property name="minimum">
-           <double>0.001000000000000</double>
-          </property>
-          <property name="maximum">
-           <double>10.000000000000000</double>
-          </property>
-          <property name="singleStep">
-           <double>0.010000000000000</double>
-          </property>
-          <property name="value">
-           <double>0.020000000000000</double>
-          </property>
-         </widget>
-        </item>
-        <item row="3" column="1">
-         <widget class="QLabel" name="label_11">
-          <property name="text">
-           <string>Minimum inliers</string>
-          </property>
-          <property name="wordWrap">
-           <bool>true</bool>
-          </property>
-         </widget>
-        </item>
-        <item row="3" column="0">
-         <widget class="QSpinBox" name="sba_minInliers">
-          <property name="minimum">
-           <number>1</number>
-          </property>
-          <property name="maximum">
-           <number>1000</number>
-          </property>
-          <property name="value">
-           <number>10</number>
-          </property>
-         </widget>
-        </item>
-        <item row="0" column="0">
          <widget class="QSpinBox" name="sba_iterations">
           <property name="minimum">
            <number>1</number>
@@ -279,17 +194,21 @@
           </property>
          </widget>
         </item>
-        <item row="1" column="1">
+        <item row="0" column="0">
-         <widget class="QLabel" name="label_9">
+         <widget class="QComboBox" name="comboBox_sbaType">
+          <item>
            <property name="text">
-           <string>Stop optimizing when the error improvement is less than this value.</string>
+            <string>g2o</string>
            </property>
-          <property name="wordWrap">
+          </item>
-           <bool>true</bool>
+          <item>
+           <property name="text">
+            <string>cvsba</string>
            </property>
+          </item>
          </widget>
         </item>
-        <item row="1" column="0">
+        <item row="2" column="0">
          <widget class="QDoubleSpinBox" name="sba_epsilon">
           <property name="suffix">
            <string/>
@@ -311,6 +230,36 @@
           </property>
          </widget>
         </item>
+        <item row="1" column="1">
+         <widget class="QLabel" name="label_8">
+          <property name="text">
+           <string>Iterations</string>
+          </property>
+          <property name="wordWrap">
+           <bool>true</bool>
+          </property>
+         </widget>
+        </item>
+        <item row="2" column="1">
+         <widget class="QLabel" name="label_9">
+          <property name="text">
+           <string>Stop optimizing when the error improvement is less than this value.</string>
+          </property>
+          <property name="wordWrap">
+           <bool>true</bool>
+          </property>
+         </widget>
+        </item>
+        <item row="0" column="1">
+         <widget class="QLabel" name="label_10">
+          <property name="text">
+           <string>Type</string>
+          </property>
+          <property name="wordWrap">
+           <bool>true</bool>
+          </property>
+         </widget>
+        </item>
        </layout>
       </item>
      </layout>