Merged master -> devel, updated default post-processing parameters

CMakeLists.txt

@@ -128,6 +128,7 @@ option(WITH_FREENECT2     "Include Freenect2 support"            ON)
 option(WITH_OPENNI2       "Include OpenNI2 support"              ON)
 option(WITH_DC1394        "Include dc1394 support"               ON)
 option(WITH_G2O           "Include g2o support"                  ON)
+option(WITH_CVSBA         "Include cvsba support"                ON)
 option(WITH_FLYCAPTURE2   "Include FlyCapture2/Triclops support" ON)
 
 FIND_PACKAGE(OpenCV REQUIRED)
@@ -173,6 +174,10 @@ IF(WITH_FLYCAPTURE2)
     FIND_PACKAGE(FlyCapture2)
 ENDIF(WITH_FLYCAPTURE2)
 
+IF(WITH_CVSBA)
+    FIND_PACKAGE(cvsba)
+ENDIF(WITH_CVSBA)
+
 ####### OSX BUNDLE CMAKE_INSTALL_PREFIX #######
 IF(APPLE AND BUILD_AS_BUNDLE)
   IF(Qt5_FOUND OR (QT4_FOUND AND QT_QTCORE_FOUND AND QT_QTGUI_FOUND))
@@ -424,6 +429,14 @@ ELSE()
 MESSAGE(STATUS "  With g2o                  = NO (g2o not found)")
 ENDIF()
 
+IF(cvsba_FOUND)
+MESSAGE(STATUS "  With cvsba                = YES")
+ELSEIF(NOT WITH_CVSBA)
+MESSAGE(STATUS "  With cvsba                = NO (WITH_CVSBA=OFF)")
+ELSE()
+MESSAGE(STATUS "  With cvsba                = NO (cvsba not found)")
+ENDIF()
+
 IF(QT4_FOUND)
 MESSAGE(STATUS "  With Qt                   = YES (version 4)")
 ELSEIF(Qt5_FOUND)

cmake_modules/FindFlyCapture2.cmake

@@ -34,9 +34,6 @@ IF (FlyCapture2_INCLUDE_DIR AND Triclops_INCLUDE_DIR AND FlyCapture2_LIBRARY AND
    SET(FlyCapture2_LIBRARIES ${FlyCapture2_LIBRARY} ${Triclops_LIBRARY} ${FlyCaptureBridge_LIBRARY} ${pnmutils_LIBRARY})
 ENDIF (FlyCapture2_INCLUDE_DIR AND Triclops_INCLUDE_DIR AND FlyCapture2_LIBRARY AND Triclops_LIBRARY AND FlyCaptureBridge_LIBRARY AND pnmutils_LIBRARY)
 
-MESSAGE(STATUS "FlyCapture2_INCLUDE_DIRS={FlyCapture2_INCLUDE_DIRS}")
-MESSAGE(STATUS "FlyCapture2_LIBRARIES={FlyCapture2_LIBRARIES}")
-
 IF (FlyCapture2_FOUND)
    # show which FlyCapture2 was found only if not quiet
    IF (NOT FlyCapture2_FIND_QUIETLY)

corelib/include/rtabmap/core/Graph.h

@@ -34,6 +34,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 #include <list>
 #include <rtabmap/core/Link.h>
 #include <rtabmap/core/Parameters.h>
+#include <rtabmap/core/Signature.h>
 
 namespace rtabmap {
 class Memory;
@@ -49,7 +50,8 @@ public:
 	enum Type {
 		kTypeUndef = -1,
 		kTypeTORO = 0,
-		kTypeG2O = 1
+		kTypeG2O = 1,
+		kTypeCVSBA = 2
 	};
 	static Optimizer * create(const ParametersMap & parameters);
 	static Optimizer * create(Optimizer::Type & type, const ParametersMap & parameters = ParametersMap());
@@ -73,11 +75,17 @@ public:
 	bool isCovarianceIgnored() const {return covarianceIgnored_;}
 	double epsilon() const {return epsilon_;}
 
+	// inherited classes should implement one of these methods
 	virtual 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) = 0;
+			std::list<std::map<int, Transform> > * intermediateGraphes = 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);
 
 	virtual void parseParameters(const ParametersMap & parameters);
 
@@ -145,6 +153,38 @@ public:
 			std::list<std::map<int, Transform> > * intermediateGraphes = 0);
 };
 
+class CVSBAOptimizer : public Optimizer
+{
+public:
+	static bool available();
+
+public:
+	CVSBAOptimizer(int iterations = 100, bool slam2d = false, bool covarianceIgnored = false) :
+		Optimizer(iterations, slam2d, covarianceIgnored),
+		inlierDistance_(0.02),
+		minInliers_(10){}
+	CVSBAOptimizer(const ParametersMap & parameters) :
+		Optimizer(parameters),
+		inlierDistance_(0.02),
+		minInliers_(10){}
+	virtual ~CVSBAOptimizer() {}
+
+	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_;
+};
+
 ////////////////////////////////////////////
 // Graph utilities
 ////////////////////////////////////////////

corelib/include/rtabmap/core/Parameters.h

@@ -314,7 +314,7 @@ class RTABMAP_EXP Parameters
 	RTABMAP_PARAM(Odom, Strategy,           	int, 0, 		"0=Bag-of-words 1=Optical Flow");
 	RTABMAP_PARAM(Odom, FeatureType,            int, 6, 	    "0=SURF 1=SIFT 2=ORB 3=FAST/FREAK 4=FAST/BRIEF 5=GFTT/FREAK 6=GFTT/BRIEF 7=BRISK.");
 	RTABMAP_PARAM(Odom, EstimationType,  		int, 0,    	    "Motion estimation approach: 0:3D->3D, 1:3D->2D (PnP)");
-	RTABMAP_PARAM(Odom, MaxFeatures,            int, 400, 		"0 no limits.");
+	RTABMAP_PARAM(Odom, MaxFeatures,            int, 1000, 		"0 no limits.");
 	RTABMAP_PARAM(Odom, InlierDistance,         float, 0.02, 	"Maximum distance for visual word correspondences.");
 	RTABMAP_PARAM(Odom, MinInliers,             int, 20, 		"Minimum visual word correspondences to compute geometry transform.");
 	RTABMAP_PARAM(Odom, Iterations,             int, 100, 		"Maximum iterations to compute the transform from visual words.");
@@ -364,8 +364,8 @@ class RTABMAP_EXP Parameters
 	RTABMAP_PARAM(LccIcp, MaxRotation,     float, 0.78,     "Maximum ICP rotation correction accepted (rad).");
 
 	RTABMAP_PARAM(LccBow, EstimationType,  int, 0,    	    "Motion estimation approach: 0:3D->3D, 1:3D->2D (PnP), 2:2D->2D (Epipolar Geometry)");
-	RTABMAP_PARAM(LccBow, MinInliers,      int, 20, 		"Minimum visual word correspondences to compute geometry transform.");
+	RTABMAP_PARAM(LccBow, MinInliers,      int, 10, 		"Minimum visual word correspondences to compute geometry transform.");
-	RTABMAP_PARAM(LccBow, InlierDistance,  float, 0.02, 	"Maximum distance for visual word correspondences.");
+	RTABMAP_PARAM(LccBow, InlierDistance,  float, 0.05, 	"Maximum distance for visual word correspondences.");
 	RTABMAP_PARAM(LccBow, Iterations,      int, 100, 		"Maximum iterations to compute the transform from visual words.");
 	RTABMAP_PARAM(LccBow, RefineIterations,  int, 10,        "Number of iterations used to refine the transformation found by RANSAC. 0 means that the transformation is not refined.");
 	RTABMAP_PARAM(LccBow, Force2D, 		   bool, false,     "Force 2D transform (3Dof: x,y and yaw).");
@@ -376,7 +376,7 @@ class RTABMAP_EXP Parameters
 	RTABMAP_PARAM(LccReextract, NNType, 	int, 3, 		"kNNFlannNaive=0, kNNFlannKdTree=1, kNNFlannLSH=2, kNNBruteForce=3, kNNBruteForceGPU=4.");
 	RTABMAP_PARAM(LccReextract, NNDR, 		float, 0.8, 	"NNDR: nearest neighbor distance ratio.");
 	RTABMAP_PARAM(LccReextract, FeatureType, int, 4, 		"0=SURF 1=SIFT 2=ORB 3=FAST/FREAK 4=FAST/BRIEF 5=GFTT/FREAK 6=GFTT/BRIEF 7=BRISK.");
-	RTABMAP_PARAM(LccReextract, MaxWords, 	int, 600, 		"0 no limits.");
+	RTABMAP_PARAM(LccReextract, MaxWords, 	int, 1000, 		"0 no limits.");
 	RTABMAP_PARAM(LccReextract, MaxDepth, 	float, 0.0,     "Max depth of the words (0 means no limit).");
 
 	RTABMAP_PARAM(LccIcp3, Decimation,      int, 8, 		"Depth image decimation.");

corelib/src/CMakeLists.txt

@@ -151,6 +151,18 @@ IF(G2O_FOUND)
 	)
 ENDIF(G2O_FOUND)
 
+IF(cvsba_FOUND)
+	ADD_DEFINITIONS("-DWITH_CVSBA")
+	SET(INCLUDE_DIRS 
+		${INCLUDE_DIRS} 
+		${cvsba_INCLUDE_DIRS}
+	)
+	SET(LIBRARIES
+		${LIBRARIES}
+		${cvsba_LIBS}
+	)
+ENDIF(cvsba_FOUND)
+
 ####################################
 # Generate resources files
 ####################################

corelib/src/Graph.cpp

@@ -54,6 +54,13 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 #include "g2o/types/slam2d/edge_se2.h"
 #endif
 
+#ifdef WITH_CVSBA
+#include <cvsba/cvsba.h>
+#include "rtabmap/core/util3d_motion_estimation.h"
+#include "rtabmap/core/util3d_transforms.h"
+#include "rtabmap/core/util3d_correspondences.h"
+#endif
+
 namespace rtabmap {
 
 namespace graph {
@@ -137,6 +144,26 @@ void Optimizer::parseParameters(const ParametersMap & parameters)
 	Parameters::parse(parameters, Parameters::kRGBDOptimizeEpsilon(), epsilon_);
 }
 
+std::map<int, Transform> Optimizer::optimize(
+		int rootId,
+		const std::map<int, Transform> & poses,
+		const std::multimap<int, Link> & constraints,
+		std::list<std::map<int, Transform> > * intermediateGraphes)
+{
+	UERROR("Optimizer %d doesn't implement optimize() method. See optimizeBA().", (int)this->type());
+	return std::map<int, Transform>();
+}
+
+std::map<int, Transform> Optimizer::optimizeBA(
+		int rootId,
+		const std::map<int, Transform> & poses,
+		const std::multimap<int, Link> & links,
+		const std::map<int, Signature> & signatures)
+{
+	UERROR("Optimizer %d doesn't implement optimizeBA() method. See optimize().", (int)this->type());
+	return std::map<int, Transform>();
+}
+
 void Optimizer::getConnectedGraph(
 		int fromId,
 		const std::map<int, Transform> & posesIn,
@@ -889,6 +916,225 @@ std::map<int, Transform> G2OOptimizer::optimize(
 	return optimizedPoses;
 }
 
+//////////////////////
+// cvsba
+//////////////////////
+bool CVSBAOptimizer::available()
+{
+#ifdef WITH_CVSBA
+	return true;
+#else
+	return false;
+#endif
+}
+
+std::map<int, Transform> CVSBAOptimizer::optimizeBA(
+		int rootId,
+		const std::map<int, Transform> & poses,
+		const std::multimap<int, Link> & links,
+		const std::map<int, Signature> & signatures)
+{
+#ifdef WITH_CVSBA
+	// run sba optimization
+	cvsba::Sba sba;
+
+	// change params if desired
+	cvsba::Sba::Params params ;
+	params.type = cvsba::Sba::MOTIONSTRUCTURE;
+	params.iterations = this->iterations();
+	params.minError = this->epsilon();
+	params.fixedIntrinsics = 5;
+	params.fixedDistortion = 5;
+	params.verbose=ULogger::level() <= ULogger::kInfo;
+	sba.setParams(params);
+
+	std::map<int, Transform> frames = poses;
+
+	std::vector<cv::Mat> cameraMatrix(frames.size()); //nframes
+	std::vector<cv::Mat> R(frames.size()); //nframes
+	std::vector<cv::Mat> T(frames.size()); //nframes
+	std::vector<cv::Mat> distCoeffs(frames.size()); //nframes
+	std::map<int, int> frameIdToIndex;
+	std::map<int, CameraModel> models;
+	int oi=0;
+	for(std::map<int, Transform>::iterator iter=frames.begin(); iter!=frames.end(); )
+	{
+		CameraModel model;
+		if(uContains(signatures, iter->first))
+		{
+			if(signatures.at(iter->first).sensorData().cameraModels().size() == 1 && signatures.at(iter->first).sensorData().cameraModels().at(0).isValid())
+			{
+				model = signatures.at(iter->first).sensorData().cameraModels()[0];
+			}
+			else if(signatures.at(iter->first).sensorData().stereoCameraModel().isValid())
+			{
+				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.isValid())
+		{
+			frameIdToIndex.insert(std::make_pair(iter->first, oi));
+
+			cameraMatrix[oi] = model.K();
+			distCoeffs[oi] = model.D();
+
+			Transform t = (iter->second * model.localTransform()).inverse();
+
+			R[oi] = (cv::Mat_<double>(3,3) <<
+					(double)t.r11(), (double)t.r12(), (double)t.r13(),
+					(double)t.r21(), (double)t.r22(), (double)t.r23(),
+					(double)t.r31(), (double)t.r32(), (double)t.r33());
+			T[oi] = (cv::Mat_<double>(1,3) << (double)t.x(), (double)t.y(), (double)t.z());
+			++oi;
+
+			models.insert(std::make_pair(iter->first, model));
+
+			UDEBUG("Pose %d = %s", iter->first, t.prettyPrint().c_str());
+
+			++iter;
+		}
+		else
+		{
+			frames.erase(iter++);
+		}
+	}
+	cameraMatrix.resize(oi);
+	R.resize(oi);
+	T.resize(oi);
+	distCoeffs.resize(oi);
+
+	std::map<int, pcl::PointXYZ> points3DMap;
+	std::multimap<int, std::pair<int, cv::Point2f> > wordReferences; // <ID words, IDs frames + keypoint>
+	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()))
+		{
+			const Signature & sFrom = signatures.at(link.from());
+			const Signature & sTo = signatures.at(link.to());
+
+			std::vector<int> inliers;
+			Transform t = util3d::estimateMotion3DTo3D(
+					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)
+				{
+					pcl::PointXYZ p = util3d::transformPoint(sFrom.getWords3().lower_bound(inliers[i])->second, pose);
+					std::map<int, pcl::PointXYZ>::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()));
+		visibility[i].resize(wordReferencesKeys.size(), 0);
+	}
+	int i=0;
+	for(std::list<int>::iterator iter = wordReferencesKeys.begin(); iter!=wordReferencesKeys.end(); ++iter)
+	{
+		pcl::PointXYZ & p = points3DMap.at(*iter);
+		points[i].x = p.x;
+		points[i].y = p.y;
+		points[i].z = p.z;
+
+		std::multimap<int, std::pair<int, cv::Point2f> >::iterator jter = wordReferences.lower_bound(*iter);
+		while(jter->first == *iter && jter != wordReferences.end())
+		{
+			imagePoints[frameIdToIndex.at(jter->second.first)][i] = jter->second.second;
+			visibility[frameIdToIndex.at(jter->second.first)][i] = 1;
+			++jter;
+		}
+
+		++i;
+	}
+
+	// SBA
+	try
+	{
+		sba.run( points, imagePoints, visibility, cameraMatrix, R, T, distCoeffs);
+	}
+	catch(cv::Exception & e)
+	{
+		UERROR("Running SBA... error! %s", e.what());
+		return std::map<int, Transform>();
+	}
+
+	//update poses
+	i=0;
+	for(std::map<int, Transform>::iterator iter=frames.begin(); iter!=frames.end(); ++iter)
+	{
+		Transform t(R[i].at<double>(0,0), R[i].at<double>(0,1), R[i].at<double>(0,2), T[i].at<double>(0),
+					R[i].at<double>(1,0), R[i].at<double>(1,1), R[i].at<double>(1,2), T[i].at<double>(1),
+					R[i].at<double>(2,0), R[i].at<double>(2,1), R[i].at<double>(2,2), T[i].at<double>(2));
+
+		UDEBUG("New pose %d = %s", iter->first, t.prettyPrint().c_str());
+
+		iter->second = (models.at(iter->first).localTransform() * t).inverse();
+
+		++i;
+	}
+
+	return frames;
+
+#else
+	UERROR("RTAB-Map is not built with cvsba!");
+	return std::map<int, Transform>();
+#endif
+}
+
 ////////////////////////////////////////////
 // Graph utilities
 ////////////////////////////////////////////

guilib/src/AboutDialog.cpp

@@ -61,6 +61,13 @@ AboutDialog::AboutDialog(QWidget * parent) :
 	_ui->label_flycapture2->setText(CameraStereoFlyCapture2::available()?"Yes":"No");
 
 	_ui->label_g2o->setText(graph::G2OOptimizer::available()?"Yes":"No");
+
+#ifdef WITH_CVSBA
+	_ui->label_cvsba->setText("Yes");
+#else
+	_ui->label_cvsba->setText("No");
+#endif
+
 }
 
 AboutDialog::~AboutDialog()

guilib/src/MainWindow.cpp

@@ -3250,8 +3250,13 @@ void MainWindow::postProcessing()
 	double clusterRadius = _postProcessingDialog->clusterRadius();
 	double clusterAngle = _postProcessingDialog->clusterAngle();
 	int detectLoopClosureIterations = _postProcessingDialog->iterations();
+	bool sba = _postProcessingDialog->isSBA();
+	int sbaIterations = _postProcessingDialog->sbaIterations();
+	double sbaEpsilon = _postProcessingDialog->sbaEpsilon();
+	double sbaInlierDistance = _postProcessingDialog->sbaInlierDistance();
+	int sbaMinInliers = _postProcessingDialog->sbaMinInliers();
 
-	if(!detectMoreLoopClosures && !refineNeighborLinks && !refineLoopClosureLinks)
+	if(!detectMoreLoopClosures && !refineNeighborLinks && !refineLoopClosureLinks && !sba)
 	{
 		UWARN("No post-processing selection...");
 		return;
@@ -3331,6 +3336,10 @@ void MainWindow::postProcessing()
 	{
 		totalSteps+=(int)_currentLinksMap.size() - (int)odomPoses.size();
 	}
+	if(sba)
+	{
+		totalSteps+=1;
+	}
 	_initProgressDialog->setMaximumSteps(totalSteps);
 	_initProgressDialog->show();
 
@@ -3676,6 +3685,33 @@ void MainWindow::postProcessing()
 	_initProgressDialog->appendText(tr("Optimizing graph with updated links... done!"));
 	_initProgressDialog->incrementStep();
 
+	if(sba)
+	{
+		_initProgressDialog->appendText(tr("SBA (%1 nodes, %2 constraints, %3 iterations)...")
+					.arg(optimizedPoses.size()).arg(linksOut.size()).arg(sbaIterations));
+		QApplication::processEvents();
+		uSleep(100);
+		QApplication::processEvents();
+
+		ParametersMap parametersSBA = _preferencesDialog->getAllParameters();
+		uInsert(parametersSBA, std::make_pair(Parameters::kRGBDOptimizeIterations(), uNumber2Str(sbaIterations)));
+		uInsert(parametersSBA, std::make_pair(Parameters::kRGBDOptimizeEpsilon(), uNumber2Str(sbaEpsilon)));
+		graph::CVSBAOptimizer cvsba = graph::CVSBAOptimizer(parametersSBA);
+		cvsba.setInlierDistance(sbaInlierDistance);
+		cvsba.setMinInliers(sbaMinInliers);
+		std::map<int, Transform>  newPoses = cvsba.optimizeBA(0, optimizedPoses, linksOut, _cachedSignatures.toStdMap());
+		if(newPoses.size())
+		{
+			optimizedPoses = newPoses;
+			_initProgressDialog->appendText(tr("SBA... done!"));
+		}
+		else
+		{
+			_initProgressDialog->appendText(tr("SBA... failed!"));
+		}
+		_initProgressDialog->incrementStep();
+	}
+
 	_initProgressDialog->appendText(tr("Updating map..."));
 	this->updateMapCloud(
 			optimizedPoses, Transform(),

guilib/src/PostProcessingDialog.cpp

@@ -29,6 +29,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 #include "ui_postProcessingDialog.h"
 
 #include <QPushButton>
+#include <rtabmap/core/Graph.h>
 
 namespace rtabmap {
 
@@ -38,9 +39,18 @@ PostProcessingDialog::PostProcessingDialog(QWidget * parent) :
 	_ui = new Ui_PostProcessingDialog();
 	_ui->setupUi(this);
 
+	if(!graph::CVSBAOptimizer::available())
+	{
+		_ui->sba->setEnabled(false);
+		_ui->sba->setChecked(false);
+	}
+
+	restoreDefaults();
+
 	connect(_ui->detectMoreLoopClosures, SIGNAL(clicked(bool)), this, SLOT(updateButtonBox()));
 	connect(_ui->refineNeighborLinks, SIGNAL(stateChanged(int)), this, SLOT(updateButtonBox()));
 	connect(_ui->refineLoopClosureLinks, SIGNAL(stateChanged(int)), this, SLOT(updateButtonBox()));
+	connect(_ui->sba, SIGNAL(clicked(bool)), this, SLOT(updateButtonBox()));
 	connect(_ui->buttonBox->button(QDialogButtonBox::RestoreDefaults), SIGNAL(clicked()), this, SLOT(restoreDefaults()));
 
 	connect(_ui->detectMoreLoopClosures, SIGNAL(clicked(bool)), this, SIGNAL(configChanged()));
@@ -50,6 +60,12 @@ PostProcessingDialog::PostProcessingDialog(QWidget * parent) :
 	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->sba_minInliers, SIGNAL(valueChanged(int)), this, SIGNAL(configChanged()));
 }
 
 PostProcessingDialog::~PostProcessingDialog()
@@ -70,6 +86,11 @@ void PostProcessingDialog::saveSettings(QSettings & settings, const QString & gr
 	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_min_inliers", this->sbaMinInliers());
 	if(!group.isEmpty())
 	{
 		settings.endGroup();
@@ -89,6 +110,11 @@ void PostProcessingDialog::loadSettings(QSettings & settings, const QString & gr
 	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->setSBAMinInliers(settings.value("sba_min_inliers", this->sbaMinInliers()).toInt());
 	if(!group.isEmpty())
 	{
 		settings.endGroup();
@@ -97,19 +123,24 @@ void PostProcessingDialog::loadSettings(QSettings & settings, const QString & gr
 
 void PostProcessingDialog::restoreDefaults()
 {
-	setDetectMoreLoopClosures(true);
+	setDetectMoreLoopClosures(false);
 	setClusterRadius(0.3);
 	setClusterAngle(30);
 	setIterations(1);
 	setReextractFeatures(false);
 	setRefineNeighborLinks(false);
 	setRefineLoopClosureLinks(false);
+	setSBA(false);
+	setSBAIterations(20);
+	setSBAEpsilon(0.0001);
+	setSBAInlierDistance(0.05);
+	setSBAMinInliers(10);
 }
 
 void PostProcessingDialog::updateButtonBox()
 {
 	_ui->buttonBox->button(QDialogButtonBox::Ok)->setEnabled(
-			isDetectMoreLoopClosures() || isRefineNeighborLinks() || isRefineLoopClosureLinks());
+			isDetectMoreLoopClosures() || isRefineNeighborLinks() || isRefineLoopClosureLinks() || isSBA());
 }
 
 bool PostProcessingDialog::isDetectMoreLoopClosures() const
@@ -147,6 +178,28 @@ bool PostProcessingDialog::isRefineLoopClosureLinks() const
 	return _ui->refineLoopClosureLinks->isChecked();
 }
 
+bool PostProcessingDialog::isSBA() const
+{
+	return _ui->sba->isChecked();
+}
+
+int PostProcessingDialog::sbaIterations() const
+{
+	return _ui->sba_iterations->value();
+}
+double PostProcessingDialog::sbaEpsilon() const
+{
+	return _ui->sba_epsilon->value();
+}
+double PostProcessingDialog::sbaInlierDistance() const
+{
+	return _ui->sba_minInlierDistance->value();
+}
+int PostProcessingDialog::sbaMinInliers() const
+{
+	return _ui->sba_minInliers->value();
+}
+
 //setters
 void PostProcessingDialog::setDetectMoreLoopClosures(bool on)
 {
@@ -176,5 +229,26 @@ void PostProcessingDialog::setRefineLoopClosureLinks(bool on)
 {
 	_ui->refineLoopClosureLinks->setChecked(on);
 }
+void PostProcessingDialog::setSBA(bool on)
+{
+	_ui->sba->setChecked(graph::CVSBAOptimizer::available() && on);
+}
+void PostProcessingDialog::setSBAIterations(int iterations)
+{
+	_ui->sba_iterations->setValue(iterations);
+}
+void PostProcessingDialog::setSBAEpsilon(double epsilon)
+{
+	_ui->sba_epsilon->setValue(epsilon);
+}
+void PostProcessingDialog::setSBAInlierDistance(double inlierDistance)
+{
+	_ui->sba_minInlierDistance->setValue(inlierDistance);
+}
+void PostProcessingDialog::setSBAMinInliers(int minInliers)
+{
+	_ui->sba_minInliers->setValue(minInliers);
+}
+
 
 }

guilib/src/PostProcessingDialog.h

@@ -56,6 +56,11 @@ public:
 	bool isReextractFeatures() const;
 	bool isRefineNeighborLinks() const;
 	bool isRefineLoopClosureLinks() const;
+	bool isSBA() const;
+	int sbaIterations() const;
+	double sbaEpsilon() const;
+	double sbaInlierDistance() const;
+	int sbaMinInliers() const;
 
 	//setters
 	void setDetectMoreLoopClosures(bool on);
@@ -65,6 +70,11 @@ public:
 	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 setSBAMinInliers(int minInliers);
 
 signals:
 	void configChanged();

guilib/src/ui/aboutDialog.ui

@@ -7,7 +7,7 @@
     <x>0</x>
     <y>0</y>
     <width>824</width>
-    <height>507</height>
+    <height>581</height>
    </rect>
   </property>
   <property name="sizePolicy">
@@ -82,6 +82,13 @@ p, li { white-space: pre-wrap; }
         </item>
         <item>
          <layout class="QGridLayout" name="gridLayout" columnstretch="0,1">
+          <item row="11" column="0">
+           <widget class="QLabel" name="label_13">
+            <property name="text">
+             <string>With OpenNI2 :</string>
+            </property>
+           </widget>
+          </item>
           <item row="13" column="0">
            <widget class="QLabel" name="label_16">
             <property name="text">
@@ -205,13 +212,6 @@ p, li { white-space: pre-wrap; }
             </property>
            </widget>
           </item>
-          <item row="11" column="0">
-           <widget class="QLabel" name="label_13">
-            <property name="text">
-             <string>With OpenNI2 :</string>
-            </property>
-           </widget>
-          </item>
           <item row="15" column="0">
            <widget class="QLabel" name="label_14">
             <property name="text">
@@ -293,6 +293,23 @@ p, li { white-space: pre-wrap; }
             </property>
            </widget>
           </item>
+          <item row="16" column="0">
+           <widget class="QLabel" name="label_18">
+            <property name="text">
+             <string>With cvsba :</string>
+            </property>
+           </widget>
+          </item>
+          <item row="16" column="1">
+           <widget class="QLabel" name="label_cvsba">
+            <property name="text">
+             <string/>
+            </property>
+            <property name="alignment">
+             <set>Qt::AlignLeading|Qt::AlignLeft|Qt::AlignVCenter</set>
+            </property>
+           </widget>
+          </item>
          </layout>
         </item>
         <item>

guilib/src/ui/mainWindow.ui

@@ -27,7 +27,7 @@
      <x>0</x>
      <y>0</y>
      <width>1012</width>
-     <height>22</height>
+     <height>25</height>
     </rect>
    </property>
    <widget class="QMenu" name="menuFile">

guilib/src/ui/postProcessingDialog.ui

@@ -6,8 +6,8 @@
    <rect>
     <x>0</x>
     <y>0</y>
-    <width>486</width>
+    <width>552</width>
-    <height>425</height>
+    <height>562</height>
    </rect>
   </property>
   <property name="windowTitle">
@@ -187,6 +187,135 @@
      </layout>
     </widget>
    </item>
+   <item>
+    <widget class="QGroupBox" name="sba">
+     <property name="title">
+      <string>Sparse Bundle Adjustment (SBA)</string>
+     </property>
+     <property name="checkable">
+      <bool>true</bool>
+     </property>
+     <property name="checked">
+      <bool>false</bool>
+     </property>
+     <layout class="QVBoxLayout" name="verticalLayout_4">
+      <item>
+       <layout class="QGridLayout" name="gridLayout_2" columnstretch="0,1">
+        <item row="0" 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_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>
+          </property>
+          <property name="maximum">
+           <number>1000</number>
+          </property>
+          <property name="value">
+           <number>100</number>
+          </property>
+         </widget>
+        </item>
+        <item row="1" 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="1" column="0">
+         <widget class="QDoubleSpinBox" name="sba_epsilon">
+          <property name="suffix">
+           <string/>
+          </property>
+          <property name="decimals">
+           <number>4</number>
+          </property>
+          <property name="minimum">
+           <double>0.000100000000000</double>
+          </property>
+          <property name="maximum">
+           <double>1.000000000000000</double>
+          </property>
+          <property name="singleStep">
+           <double>0.000100000000000</double>
+          </property>
+          <property name="value">
+           <double>0.000100000000000</double>
+          </property>
+         </widget>
+        </item>
+       </layout>
+      </item>
+     </layout>
+    </widget>
+   </item>
    <item>
     <spacer name="verticalSpacer">
      <property name="orientation">

utilite/include/rtabmap/utilite/UStl.h

@@ -472,6 +472,7 @@ inline std::list<V> uVectorToList(const std::vector<V> & v)
 
 /**
  * Convert a std::multimap to a std::map
+ * @see uMultimapToMapUnique to keep only unique keys
  */
 template<class K, class V>
 inline std::map<K, V> uMultimapToMap(const std::multimap<K, V> & m)
@@ -479,6 +480,25 @@ inline std::map<K, V> uMultimapToMap(const std::multimap<K, V> & m)
 	return std::map<K, V>(m.begin(), m.end());
 }
 
+/**
+ * Convert a std::multimap to a std::map, keeping only unique keys!
+ */
+template<class K, class V>
+inline std::map<K, V> uMultimapToMapUnique(const std::multimap<K, V> & m)
+{
+	std::map<K, V> mapOut;
+	std::list<K> uniqueKeys = uUniqueKeys(m);
+	for(typename std::list<K>::const_iterator iter = uniqueKeys.begin(); iter!=uniqueKeys.end(); ++iter)
+	{
+		if(m.count(*iter) == 1)
+		{
+			typename std::multimap<K, V>::const_iterator jter=m.find(*iter);
+			mapOut.insert(std::pair<K,V>(jter->first, jter->second));
+		}
+	}
+	return mapOut;
+}
+
 /**
  * Append a list to another list.
  * @param list the list on which the other list will be appended