Added parameter "RGBD/OptimizeRobust" (default true) to use Vertigo robust graph optimization (only for g2o and GTSAM optimization strategies). Added GTSAM support.

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_GTSAM         "Include GTSAM support"                ON)
 option(WITH_CVSBA         "Include cvsba support"                ON)
 option(WITH_FLYCAPTURE2   "Include FlyCapture2/Triclops support" ON)
 
@@ -170,6 +171,10 @@ IF(WITH_G2O)
     FIND_PACKAGE(G2O)
 ENDIF(WITH_G2O)
 
+IF(WITH_GTSAM)
+    FIND_PACKAGE(GTSAM)
+ENDIF(WITH_GTSAM)
+
 IF(WITH_FLYCAPTURE2)
     FIND_PACKAGE(FlyCapture2)
 ENDIF(WITH_FLYCAPTURE2)
@@ -429,6 +434,14 @@ ELSE()
 MESSAGE(STATUS "  With g2o                  = NO (g2o not found)")
 ENDIF()
 
+IF(GTSAM_FOUND)
+MESSAGE(STATUS "  With GTSAM                = YES")
+ELSEIF(NOT WITH_GTSAM)
+MESSAGE(STATUS "  With GTSAM                = NO (WITH_GTSAM=OFF)")
+ELSE()
+MESSAGE(STATUS "  With GTSAM                = NO (GTSAM not found)")
+ENDIF()
+
 IF(cvsba_FOUND)
 MESSAGE(STATUS "  With cvsba                = YES")
 ELSEIF(NOT WITH_CVSBA)

app/src/main.cpp

@@ -41,7 +41,7 @@ int main(int argc, char* argv[])
 {
 	/* Set logger type */
 	ULogger::setType(ULogger::kTypeConsole);
-	ULogger::setLevel(ULogger::kInfo);
+	ULogger::setLevel(ULogger::kDebug);
 
 	/* Create tasks */
 	QApplication * app = new QApplication(argc, argv);

cmake_modules/FindG2O.cmake

@@ -70,11 +70,13 @@ IF(G2O_STUFF_LIBRARY AND G2O_CORE_LIBRARY AND G2O_INCLUDE_DIR AND G2O_SOLVERS_FO
   SET(G2O_INCLUDE_DIRS ${G2O_INCLUDE_DIR} ${CSPARSE_INCLUDE_DIR})
   SET(G2O_LIBRARIES 
 	${G2O_STUFF_LIBRARY} 
-	${G2O_CORE_LIBRARY} 
+	${G2O_CORE_LIBRARY}
+	${G2O_SOLVER_CHOLMOD}
 	${G2O_SOLVER_CSPARSE} 
 	${G2O_SOLVER_CSPARSE_EXTENSION}
 	${G2O_TYPES_SLAM2D} 
 	${G2O_TYPES_SLAM3D} 
-	${CSPARSE_LIBRARY})
+	${CSPARSE_LIBRARY}
+        cholmod)
   SET(G2O_FOUND "YES")
 ENDIF(G2O_STUFF_LIBRARY AND G2O_CORE_LIBRARY AND G2O_INCLUDE_DIR AND G2O_SOLVERS_FOUND AND CSPARSE_FOUND)

corelib/include/rtabmap/core/Graph.h

@@ -51,7 +51,8 @@ public:
 		kTypeUndef = -1,
 		kTypeTORO = 0,
 		kTypeG2O = 1,
-		kTypeCVSBA = 2
+		kTypeGTSAM = 2,
+		kTypeCVSBA = 3
 	};
 	static Optimizer * create(const ParametersMap & parameters);
 	static Optimizer * create(Optimizer::Type & type, const ParametersMap & parameters = ParametersMap());
@@ -74,6 +75,7 @@ public:
 	bool isSlam2d() const {return slam2d_;}
 	bool isCovarianceIgnored() const {return covarianceIgnored_;}
 	double epsilon() const {return epsilon_;}
+	bool isRobust() const {return robust_;}
 
 	// inherited classes should implement one of these methods
 	virtual std::map<int, Transform> optimize(
@@ -94,7 +96,8 @@ protected:
 			int iterations         = Parameters::defaultRGBDOptimizeIterations(),
 			bool slam2d            = Parameters::defaultRGBDOptimizeSlam2D(),
 			bool covarianceIgnored = Parameters::defaultRGBDOptimizeVarianceIgnored(),
-			double epsilon         = Parameters::defaultRGBDOptimizeEpsilon());
+			double epsilon         = Parameters::defaultRGBDOptimizeEpsilon(),
+			bool robust            = Parameters::defaultRGBDOptimizeRobust());
 	Optimizer(const ParametersMap & parameters);
 
 private:
@@ -102,6 +105,7 @@ private:
 	bool slam2d_;
 	bool covarianceIgnored_;
 	double epsilon_;
+	bool robust_;
 };
 
 class RTABMAP_EXP TOROOptimizer : public Optimizer
@@ -117,8 +121,12 @@ public:
 			std::multimap<int, Link> & edgeConstraints);
 
 public:
-	TOROOptimizer(int iterations = 100, bool slam2d = false, bool covarianceIgnored = false) :
-		Optimizer(iterations, slam2d, covarianceIgnored) {}
+	TOROOptimizer(
+			int iterations         = Parameters::defaultRGBDOptimizeIterations(),
+			bool slam2d            = Parameters::defaultRGBDOptimizeSlam2D(),
+			bool covarianceIgnored = Parameters::defaultRGBDOptimizeVarianceIgnored(),
+			double epsilon         = Parameters::defaultRGBDOptimizeEpsilon()) :
+		Optimizer(iterations, slam2d, covarianceIgnored, epsilon) {}
 	TOROOptimizer(const ParametersMap & parameters) :
 		Optimizer(parameters) {}
 	virtual ~TOROOptimizer() {}
@@ -136,10 +144,21 @@ class RTABMAP_EXP G2OOptimizer : public Optimizer
 {
 public:
 	static bool available();
+	static bool saveGraph(
+			const std::string & fileName,
+			const std::map<int, Transform> & poses,
+			const std::multimap<int, Link> & edgeConstraints,
+			bool useRobustConstraints = false);
 
 public:
-	G2OOptimizer(int iterations = 100, bool slam2d = false, bool covarianceIgnored = false) :
-		Optimizer(iterations, slam2d, covarianceIgnored) {}
+	G2OOptimizer(
+			int iterations         = Parameters::defaultRGBDOptimizeIterations(),
+			bool slam2d            = Parameters::defaultRGBDOptimizeSlam2D(),
+			bool covarianceIgnored = Parameters::defaultRGBDOptimizeVarianceIgnored(),
+			double epsilon         = Parameters::defaultRGBDOptimizeEpsilon(),
+			bool robust            = Parameters::defaultRGBDOptimizeRobust()) :
+		Optimizer(iterations, slam2d, covarianceIgnored, epsilon, robust) {}
+
 	G2OOptimizer(const ParametersMap & parameters) :
 		Optimizer(parameters) {}
 	virtual ~G2OOptimizer() {}
@@ -153,6 +172,33 @@ public:
 			std::list<std::map<int, Transform> > * intermediateGraphes = 0);
 };
 
+class RTABMAP_EXP GTSAMOptimizer : public Optimizer
+{
+public:
+	static bool available();
+
+public:
+	GTSAMOptimizer(
+			int iterations         = Parameters::defaultRGBDOptimizeIterations(),
+			bool slam2d            = Parameters::defaultRGBDOptimizeSlam2D(),
+			bool covarianceIgnored = Parameters::defaultRGBDOptimizeVarianceIgnored(),
+			double epsilon         = Parameters::defaultRGBDOptimizeEpsilon(),
+			bool robust            = Parameters::defaultRGBDOptimizeRobust()) :
+		Optimizer(iterations, slam2d, covarianceIgnored, epsilon, robust) {}
+
+	GTSAMOptimizer(const ParametersMap & parameters) :
+		Optimizer(parameters) {}
+	virtual ~GTSAMOptimizer() {}
+
+	virtual Type type() const {return kTypeGTSAM;}
+
+	virtual std::map<int, Transform> optimize(
+			int rootId,
+			const std::map<int, Transform> & poses,
+			const std::multimap<int, Link> & edgeConstraints,
+			std::list<std::map<int, Transform> > * intermediateGraphes = 0);
+};
+
 class RTABMAP_EXP CVSBAOptimizer : public Optimizer
 {
 public:

corelib/include/rtabmap/core/Parameters.h

@@ -306,10 +306,11 @@ class RTABMAP_EXP Parameters
 
 	// Graph optimization
 	RTABMAP_PARAM(RGBD, OptimizeStrategy,          int, 0,        "Graph optimization strategy: 0=TORO and 1=g2o.");
-	RTABMAP_PARAM(RGBD, OptimizeIterations,        int, 100,      "Optimization iterations.");
+	RTABMAP_PARAM(RGBD, OptimizeIterations,        int, 10,      "Optimization iterations.");
 	RTABMAP_PARAM(RGBD, OptimizeSlam2D,            bool, false,  "If optimization is done only on x,y and theta (3DoF). Otherwise, it is done on full 6DoF poses.");
 	RTABMAP_PARAM(RGBD, OptimizeVarianceIgnored,   bool, false,  "Ignore constraints' variance. If checked, identity information matrix is used for each constraint. Otherwise, an information matrix is generated from the variance saved in the links.");
-	RTABMAP_PARAM(RGBD, OptimizeEpsilon,           double, 0.0001,  "Stop optimizing when the error improvement is less than this value.");
+	RTABMAP_PARAM(RGBD, OptimizeEpsilon,           double, 0.001,  "Stop optimizing when the error improvement is less than this value.");
+	RTABMAP_PARAM(RGBD, OptimizeRobust,            bool, true,  "Robust graph optimization using Vertigo (only work for g2o and GTSAM optimization strategies).");
 
 	// Odometry
 	RTABMAP_PARAM(Odom, Strategy,           	int, 0, 		"0=Bag-of-words 1=Optical Flow");

corelib/include/rtabmap/core/RtabmapEvent.h

@@ -65,7 +65,7 @@ public:
 			kCmdDumpMemory,
 			kCmdDumpPrediction,
 			kCmdGenerateDOTGraph, // params: [bool] global, [string] path, if global=false: [int] id, [int] margin
-			kCmdExportPoses,      // params: [bool] global, [bool] optimized, [string] path, [int] type (0=raw format, 1=RGBD-SLAM format, 2=KITTI format, 3=TORO)
+			kCmdExportPoses,      // params: [bool] global, [bool] optimized, [string] path, [int] type (0=raw format, 1=RGBD-SLAM format, 2=KITTI format, 3=TORO, 4=g2o)
 			kCmdCleanDataBuffer,
 			kCmdPublish3DMap,     // params: [bool] global, [bool] optimized, [bool] graphOnly
 			kCmdTriggerNewMap,

corelib/include/rtabmap/core/Transform.h

@@ -51,6 +51,8 @@ public:
 	Transform(const cv::Mat & transformationMatrix);
 	// x,y,z, roll,pitch,yaw
 	Transform(float x, float y, float z, float roll, float pitch, float yaw);
+	// x,y, theta
+	Transform(float x, float y, float theta);
 
 	float r11() const {return data()[0];}
 	float r12() const {return data()[1];}

corelib/src/CMakeLists.txt

@@ -149,8 +149,34 @@ IF(G2O_FOUND)
 		${LIBRARIES}
 		${G2O_LIBRARIES}
 	)
+	#Newest versions require std11
+	SET(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -std=c++11")
+	
+	IF(WITH_VERTIGO)
+		SET(SRC_FILES
+	    	${SRC_FILES}
+			vertigo/g2o/edge_se2MaxMixture.cpp
+			vertigo/g2o/edge_se2Switchable.cpp
+			vertigo/g2o/edge_se3Switchable.cpp
+			vertigo/g2o/edge_switchPrior.cpp
+			vertigo/g2o/types_g2o_robust.cpp
+			vertigo/g2o/vertex_switchLinear.cpp
+		)
+	ENDIF(WITH_VERTIGO)
 ENDIF(G2O_FOUND)
 
+IF(GTSAM_FOUND)
+	ADD_DEFINITIONS("-DWITH_GTSAM")
+	SET(INCLUDE_DIRS 
+		${INCLUDE_DIRS} 
+		${GTSAM_INCLUDE_DIRS}
+	)
+	SET(LIBRARIES
+		${LIBRARIES}
+		gtsam
+	)
+ENDIF(GTSAM_FOUND)
+
 IF(cvsba_FOUND)
 	ADD_DEFINITIONS("-DWITH_CVSBA")
 	SET(INCLUDE_DIRS 

corelib/src/Graph.cpp

@@ -48,11 +48,50 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 #include "g2o/core/optimization_algorithm_gauss_newton.h"
 #include "g2o/core/optimization_algorithm_levenberg.h"
 #include "g2o/solvers/csparse/linear_solver_csparse.h"
+#include "g2o/solvers/cholmod/linear_solver_cholmod.h"
+#include "g2o/solvers/pcg/linear_solver_pcg.h"
 #include "g2o/types/slam3d/vertex_se3.h"
 #include "g2o/types/slam3d/edge_se3.h"
 #include "g2o/types/slam2d/vertex_se2.h"
 #include "g2o/types/slam2d/edge_se2.h"
-#endif
+
+typedef g2o::BlockSolver< g2o::BlockSolverTraits<3, 3> > Slam2dBlockSolver;
+typedef g2o::LinearSolverCSparse<Slam2dBlockSolver::PoseMatrixType> Slam2dLinearCSparseSolver;
+typedef g2o::LinearSolverCholmod<Slam2dBlockSolver::PoseMatrixType> Slam2dLinearCholmodSolver;
+typedef g2o::LinearSolverPCG<Slam2dBlockSolver::PoseMatrixType> Slam2dLinearPCGSolver;
+
+typedef g2o::BlockSolver< g2o::BlockSolverTraits<6, 3> > Slam3dBlockSolver;
+typedef g2o::LinearSolverCSparse<Slam3dBlockSolver::PoseMatrixType> Slam3dLinearCSparseSolver;
+typedef g2o::LinearSolverCholmod<Slam3dBlockSolver::PoseMatrixType> Slam3dLinearCholmodSolver;
+typedef g2o::LinearSolverPCG<Slam3dBlockSolver::PoseMatrixType> Slam3dLinearPCGSolver;
+
+#include "vertigo/g2o/edge_switchPrior.h"
+#include "vertigo/g2o/edge_se2Switchable.h"
+#include "vertigo/g2o/edge_se3Switchable.h"
+#include "vertigo/g2o/vertex_switchLinear.h"
+
+#endif // end WITH_G2O
+
+#ifdef WITH_GTSAM
+#include <gtsam/geometry/Pose2.h>
+#include <gtsam/geometry/Pose3.h>
+#include <gtsam/inference/Key.h>
+#include <gtsam/inference/Symbol.h>
+#include <gtsam/slam/PriorFactor.h>
+#include <gtsam/slam/BetweenFactor.h>
+#include <gtsam/nonlinear/NonlinearFactorGraph.h>
+#include <gtsam/nonlinear/GaussNewtonOptimizer.h>
+#include <gtsam/nonlinear/DoglegOptimizer.h>
+#include <gtsam/nonlinear/LevenbergMarquardtOptimizer.h>
+#include <gtsam/nonlinear/NonlinearOptimizer.h>
+#include <gtsam/nonlinear/Marginals.h>
+#include <gtsam/nonlinear/Values.h>
+
+#include "vertigo/gtsam/betweenFactorMaxMix.h"
+#include "vertigo/gtsam/betweenFactorSwitchable.h"
+#include "vertigo/gtsam/switchVariableLinear.h"
+#include "vertigo/gtsam/switchVariableSigmoid.h"
+#endif // end WITH_GTSAM
 
 #ifdef WITH_CVSBA
 #include <cvsba/cvsba.h>
@@ -80,16 +119,23 @@ Optimizer * Optimizer::create(const ParametersMap & parameters)
 		UWARN("g2o optimizer not available. TORO will be used instead.");
 		type = Optimizer::kTypeTORO;
 	}
+	if(!GTSAMOptimizer::available() && type == Optimizer::kTypeGTSAM)
+	{
+		UWARN("GTSAM optimizer not available. TORO will be used instead.");
+		type = Optimizer::kTypeTORO;
+	}
 	Optimizer * optimizer = 0;
 	switch(type)
 	{
+	case Optimizer::kTypeGTSAM:
+		optimizer = new GTSAMOptimizer(parameters);
+		break;
 	case Optimizer::kTypeG2O:
 		optimizer = new G2OOptimizer(parameters);
 		break;
 	case Optimizer::kTypeTORO:
 	default:
 		optimizer = new TOROOptimizer(parameters);
-		type = Optimizer::kTypeTORO;
 		break;
 
 	}
@@ -103,9 +149,17 @@ Optimizer * Optimizer::create(Optimizer::Type & type, const ParametersMap & para
 		UWARN("g2o optimizer not available. TORO will be used instead.");
 		type = Optimizer::kTypeTORO;
 	}
+	if(!GTSAMOptimizer::available() && type == Optimizer::kTypeGTSAM)
+	{
+		UWARN("GTSAM optimizer not available. TORO will be used instead.");
+		type = Optimizer::kTypeTORO;
+	}
 	Optimizer * optimizer = 0;
 	switch(type)
 	{
+	case Optimizer::kTypeGTSAM:
+		optimizer = new GTSAMOptimizer(parameters);
+		break;
 	case Optimizer::kTypeG2O:
 		optimizer = new G2OOptimizer(parameters);
 		break;
@@ -119,11 +173,12 @@ Optimizer * Optimizer::create(Optimizer::Type & type, const ParametersMap & para
 	return optimizer;
 }
 
-Optimizer::Optimizer(int iterations, bool slam2d, bool covarianceIgnored, double epsilon) :
+Optimizer::Optimizer(int iterations, bool slam2d, bool covarianceIgnored, double epsilon, bool robust) :
 		iterations_(iterations),
 		slam2d_(slam2d),
 		covarianceIgnored_(covarianceIgnored),
-		epsilon_(epsilon)
+		epsilon_(epsilon),
+		robust_(robust)
 {
 }
 
@@ -131,7 +186,8 @@ Optimizer::Optimizer(const ParametersMap & parameters) :
 		iterations_(Parameters::defaultRGBDOptimizeIterations()),
 		slam2d_(Parameters::defaultRGBDOptimizeSlam2D()),
 		covarianceIgnored_(Parameters::defaultRGBDOptimizeVarianceIgnored()),
-		epsilon_(Parameters::defaultRGBDOptimizeEpsilon())
+		epsilon_(Parameters::defaultRGBDOptimizeEpsilon()),
+		robust_(Parameters::defaultRGBDOptimizeRobust())
 {
 	parseParameters(parameters);
 }
@@ -142,6 +198,7 @@ void Optimizer::parseParameters(const ParametersMap & parameters)
 	Parameters::parse(parameters, Parameters::kRGBDOptimizeVarianceIgnored(), covarianceIgnored_);
 	Parameters::parse(parameters, Parameters::kRGBDOptimizeSlam2D(), slam2d_);
 	Parameters::parse(parameters, Parameters::kRGBDOptimizeEpsilon(), epsilon_);
+	Parameters::parse(parameters, Parameters::kRGBDOptimizeRobust(), robust_);
 }
 
 std::map<int, Transform> Optimizer::optimize(
@@ -373,9 +430,12 @@ std::map<int, Transform> TOROOptimizer::optimize(
 			pg3.initializeOptimization();
 		}
 
-		UINFO("TORO iterate begin (iterations=%d)", iterations());
+		UINFO("TORO optimizing begin (iterations=%d)", iterations());
 		double lasterror = 0;
-		for (int i=0; i<iterations(); i++)
+		double errorDelta = 0;
+		int i=0;
+		UTimer timer;
+		for (; i<iterations(); i++)
 		{
 			if(intermediateGraphes && i>0)
 			{
@@ -429,7 +489,7 @@ std::map<int, Transform> TOROOptimizer::optimize(
 			}
 
 			// early stop condition
-			double errorDelta = lasterror - error;
+			errorDelta = lasterror - error;
 			if(i>0 && errorDelta < this->epsilon())
 			{
 				UDEBUG("Stop optimizing, not enough improvement (%f < %f)", errorDelta, this->epsilon());
@@ -437,7 +497,7 @@ std::map<int, Transform> TOROOptimizer::optimize(
 			}
 			lasterror = error;
 		}
-		UINFO("TORO iterate end");
+		UINFO("TORO optimizing end (%d iterations done, error=%f, time = %f s)", i, errorDelta, timer.ticks());
 
 		if(isSlam2d())
 		{
@@ -670,20 +730,77 @@ std::map<int, Transform> G2OOptimizer::optimize(
 	{
 		// Apply g2o optimization
 
-		// create the linear solver
-		g2o::BlockSolverX::LinearSolverType * linearSolver = new g2o::LinearSolverCSparse<g2o::BlockSolverX::PoseMatrixType>();
-
-		// create the block solver on top of the linear solver
-		g2o::BlockSolverX* blockSolver = new g2o::BlockSolverX(linearSolver);
-
-		// create the algorithm to carry out the optimization
-		//g2o::OptimizationAlgorithmGaussNewton* optimizationAlgorithm = new g2o::OptimizationAlgorithmGaussNewton(blockSolver);
-		g2o::OptimizationAlgorithmLevenberg* optimizationAlgorithm = new g2o::OptimizationAlgorithmLevenberg(blockSolver);
-
-		// create the optimizer to load the data and carry out the optimization
 		g2o::SparseOptimizer optimizer;
 		optimizer.setVerbose(false);
-		optimizer.setAlgorithm(optimizationAlgorithm);
+		int solverApproach = 0;
+		int optimizationApproach = 0;
+		if(isSlam2d())
+		{
+			Slam2dBlockSolver * blockSolver;
+			if(solverApproach == 1)
+			{
+				//pcg
+				Slam2dLinearPCGSolver * linearSolver = new Slam2dLinearPCGSolver();
+				blockSolver = new Slam2dBlockSolver(linearSolver);
+			}
+			else if(solverApproach == 2)
+			{
+				//csparse
+				Slam2dLinearCSparseSolver* linearSolver = new Slam2dLinearCSparseSolver();
+				linearSolver->setBlockOrdering(false);
+				blockSolver = new Slam2dBlockSolver(linearSolver);
+			}
+			else
+			{
+				//chmold
+				Slam2dLinearCholmodSolver * linearSolver = new Slam2dLinearCholmodSolver();
+				linearSolver->setBlockOrdering(false);
+				blockSolver = new Slam2dBlockSolver(linearSolver);
+			}
+
+			if(optimizationApproach == 1)
+			{
+				optimizer.setAlgorithm(new g2o::OptimizationAlgorithmGaussNewton(blockSolver));
+			}
+			else
+			{
+				optimizer.setAlgorithm(new g2o::OptimizationAlgorithmLevenberg(blockSolver));
+			}
+		}
+		else
+		{
+			Slam3dBlockSolver * blockSolver;
+			if(solverApproach == 1)
+			{
+				//pcg
+				Slam3dLinearPCGSolver * linearSolver = new Slam3dLinearPCGSolver();
+				blockSolver = new Slam3dBlockSolver(linearSolver);
+			}
+			else if(solverApproach == 2)
+			{
+				//csparse
+				Slam3dLinearCSparseSolver* linearSolver = new Slam3dLinearCSparseSolver();
+				linearSolver->setBlockOrdering(false);
+				blockSolver = new Slam3dBlockSolver(linearSolver);
+			}
+			else
+			{
+				//chmold
+				Slam3dLinearCholmodSolver * linearSolver = new Slam3dLinearCholmodSolver();
+				linearSolver->setBlockOrdering(false);
+				blockSolver = new Slam3dBlockSolver(linearSolver);
+			}
+
+			if(optimizationApproach == 1)
+			{
+				optimizer.setAlgorithm(new g2o::OptimizationAlgorithmGaussNewton(blockSolver));
+			}
+			else
+			{
+				optimizer.setAlgorithm(new g2o::OptimizationAlgorithmLevenberg(blockSolver));
+			}
+		}
+
 
 		UDEBUG("fill poses to g2o...");
 		for(std::map<int, Transform>::const_iterator iter = poses.begin(); iter!=poses.end(); ++iter)
@@ -694,16 +811,25 @@ std::map<int, Transform> G2OOptimizer::optimize(
 			{
 				g2o::VertexSE2 * v2 = new g2o::VertexSE2();
 				v2->setEstimate(g2o::SE2(iter->second.x(), iter->second.y(), iter->second.theta()));
+				if(iter->first == rootId)
+				{
+					v2->setFixed(true);
+				}
 				vertex = v2;
 			}
 			else
 			{
 				g2o::VertexSE3 * v3 = new g2o::VertexSE3();
-				Eigen::Isometry3d pose;
+
 				Eigen::Affine3d a = iter->second.toEigen3d();
+				Eigen::Isometry3d pose;
+				pose = a.rotation();
 				pose.translation() = a.translation();
-				pose.linear() = a.rotation();
 				v3->setEstimate(pose);
+				if(iter->first == rootId)
+				{
+					v3->setFixed(true);
+				}
 				vertex = v3;
 			}
 			vertex->setId(iter->first);
@@ -711,6 +837,7 @@ std::map<int, Transform> G2OOptimizer::optimize(
 		}
 
 		UDEBUG("fill edges to g2o...");
+		int vertigoVertexId = poses.rbegin()->first+1;
 		for(std::multimap<int, Link>::const_iterator iter=edgeConstraints.begin(); iter!=edgeConstraints.end(); ++iter)
 		{
 			int id1 = iter->first;
@@ -720,6 +847,32 @@ std::map<int, Transform> G2OOptimizer::optimize(
 
 			g2o::HyperGraph::Edge * edge = 0;
 
+			VertexSwitchLinear * v = 0;
+			if(this->isRobust() && iter->second.type() != Link::kNeighbor)
+			{
+				// For loop closure links, add switchable edges
+
+				// create new switch variable
+				// Sunderhauf IROS 2012:
+				// "Since it is reasonable to initially accept all loop closure constraints,
+				//  a proper and convenient initial value for all switch variables would be
+				//  sij = 1 when using the linear switch function"
+				v = new VertexSwitchLinear();
+				v->setEstimate(1.0);
+				v->setId(vertigoVertexId++);
+				UASSERT_MSG(optimizer.addVertex(v), uFormat("cannot insert switchable vertex %d!?", v->id()).c_str());
+
+				// create switch prior factor
+				// "If the front-end is not able to assign sound individual values
+				//  for Ξij , it is save to set all Ξij = 1, since this value is close
+				//  to the individual optimal choice of Ξij for a large range of
+				//  outliers."
+				EdgeSwitchPrior * prior = new EdgeSwitchPrior();
+				prior->setMeasurement(1.0);
+				prior->setVertex(0, v);
+				UASSERT_MSG(optimizer.addEdge(prior), uFormat("cannot insert switchable prior edge %d!?", v->id()).c_str());
+			}
+
 			if(isSlam2d())
 			{
 				Eigen::Matrix<double, 3, 3> information = Eigen::Matrix<double, 3, 3>::Identity();
@@ -736,16 +889,33 @@ std::map<int, Transform> G2OOptimizer::optimize(
 					information(2,2) = iter->second.infMatrix().at<double>(5,5); // theta-theta
 				}
 
-				g2o::EdgeSE2 * e = new g2o::EdgeSE2();
-				g2o::VertexSE2* v1 = (g2o::VertexSE2*)optimizer.vertex(id1);
-				g2o::VertexSE2* v2 = (g2o::VertexSE2*)optimizer.vertex(id2);
-				UASSERT(v1 != 0);
-				UASSERT(v2 != 0);
-				e->setVertex(0, v1);
-				e->setVertex(1, v2);
-				e->setMeasurement(g2o::SE2(iter->second.transform().x(), iter->second.transform().y(), iter->second.transform().theta()));
-				e->setInformation(information);
-				edge = e;
+				if(this->isRobust() && iter->second.type() != Link::kNeighbor)
+				{
+					EdgeSE2Switchable * e = new EdgeSE2Switchable();
+					g2o::VertexSE2* v1 = (g2o::VertexSE2*)optimizer.vertex(id1);
+					g2o::VertexSE2* v2 = (g2o::VertexSE2*)optimizer.vertex(id2);
+					UASSERT(v1 != 0);
+					UASSERT(v2 != 0);
+					e->setVertex(0, v1);
+					e->setVertex(1, v2);
+					e->setVertex(2, v);
+					e->setMeasurement(g2o::SE2(iter->second.transform().x(), iter->second.transform().y(), iter->second.transform().theta()));
+					e->setInformation(information);
+					edge = e;
+				}
+				else
+				{
+					g2o::EdgeSE2 * e = new g2o::EdgeSE2();
+					g2o::VertexSE2* v1 = (g2o::VertexSE2*)optimizer.vertex(id1);
+					g2o::VertexSE2* v2 = (g2o::VertexSE2*)optimizer.vertex(id2);
+					UASSERT(v1 != 0);
+					UASSERT(v2 != 0);
+					e->setVertex(0, v1);
+					e->setVertex(1, v2);
+					e->setMeasurement(g2o::SE2(iter->second.transform().x(), iter->second.transform().y(), iter->second.transform().theta()));
+					e->setInformation(information);
+					edge = e;
+				}
 			}
 			else
 			{
@@ -757,19 +927,36 @@ std::map<int, Transform> G2OOptimizer::optimize(
 
 				Eigen::Affine3d a = iter->second.transform().toEigen3d();
 				Eigen::Isometry3d constraint;
+				constraint = a.rotation();
 				constraint.translation() = a.translation();
-				constraint.linear() = a.rotation();
 
-				g2o::EdgeSE3 * e = new g2o::EdgeSE3();
-				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(constraint);
-				e->setInformation(information);
-				edge = e;
+				if(this->isRobust() && iter->second.type() != Link::kNeighbor)
+				{
+					EdgeSE3Switchable * e = new EdgeSE3Switchable();
+					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->setVertex(2, v);
+					e->setMeasurement(constraint);
+					e->setInformation(information);
+					edge = e;
+				}
+				else
+				{
+					g2o::EdgeSE3 * e = new g2o::EdgeSE3();
+					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(constraint);
+					e->setInformation(information);
+					edge = e;
+				}
 			}
 
 			if (!optimizer.addEdge(edge))
@@ -780,25 +967,13 @@ std::map<int, Transform> G2OOptimizer::optimize(
 		}
 
 		UDEBUG("Initial optimization...");
-		UASSERT(uContains(poses, rootId));
-		if(isSlam2d())
-		{
-			g2o::VertexSE2* firstRobotPose = (g2o::VertexSE2*)optimizer.vertex(rootId);
-			UASSERT(firstRobotPose != 0);
-			firstRobotPose->setFixed(true);
-		}
-		else
-		{
-			g2o::VertexSE3* firstRobotPose = (g2o::VertexSE3*)optimizer.vertex(rootId);
-			UASSERT(firstRobotPose != 0);
-			firstRobotPose->setFixed(true);
-		}
+		optimizer.initializeOptimization();
 
-		UINFO("g2o iterate begin (max iterations=%d)", iterations());
+		UINFO("g2o optimizing begin (max iterations=%d, robust=%d)", iterations(), isRobust()?1:0);
 		int it = 0;
+		UTimer timer;
 		if(intermediateGraphes)
 		{
-			optimizer.initializeOptimization();
 			for(int i=0; i<iterations(); ++i)
 			{
 				if(i > 0)
@@ -847,18 +1022,17 @@ std::map<int, Transform> G2OOptimizer::optimize(
 				if(ULogger::level() == ULogger::kDebug)
 				{
 					optimizer.computeActiveErrors();
-					UDEBUG("iteration %d: %d nodes, %d edges, chi2: %f", i, (int)optimizer.vertices().size(), (int)optimizer.edges().size(), optimizer.chi2());
+					UDEBUG("iteration %d: %d nodes, %d edges, chi2: %f", i, (int)optimizer.vertices().size(), (int)optimizer.edges().size(), optimizer.activeRobustChi2());
 				}
 			}
 		}
 		else
 		{
-			optimizer.initializeOptimization();
 			it = optimizer.optimize(iterations());
 			optimizer.computeActiveErrors();
-			UDEBUG("%d nodes, %d edges, chi2: %f", (int)optimizer.vertices().size(), (int)optimizer.edges().size(), optimizer.chi2());
+			UDEBUG("%d nodes, %d edges, chi2: %f", (int)optimizer.vertices().size(), (int)optimizer.edges().size(), optimizer.activeRobustChi2());
 		}
-		UINFO("g2o iterate end (%d iterations done)", it);
+		UINFO("g2o optimizing end (%d iterations done, error=%f, time = %f s)", it, optimizer.activeRobustChi2(), timer.ticks());
 
 		if(isSlam2d())
 		{
@@ -916,6 +1090,312 @@ std::map<int, Transform> G2OOptimizer::optimize(
 	return optimizedPoses;
 }
 
+bool G2OOptimizer::saveGraph(
+		const std::string & fileName,
+		const std::map<int, Transform> & poses,
+		const std::multimap<int, Link> & edgeConstraints,
+		bool useRobustConstraints)
+{
+	FILE * file = 0;
+
+#ifdef _MSC_VER
+	fopen_s(&file, fileName.c_str(), "w");
+#else
+	file = fopen(fileName.c_str(), "w");
+#endif
+
+	if(file)
+	{
+		// VERTEX_SE3 id x y z qw qx qy qz
+		for(std::map<int, Transform>::const_iterator iter = poses.begin(); iter!=poses.end(); ++iter)
+		{
+			Eigen::Quaternionf q = iter->second.getQuaternionf();
+			fprintf(file, "VERTEX_SE3:QUAT %d %f %f %f %f %f %f %f\n",
+					iter->first,
+					iter->second.x(),
+					iter->second.y(),
+					iter->second.z(),
+					q.x(),
+					q.y(),
+					q.z(),
+					q.w());
+		}
+
+		//EDGE_SE3 observed_vertex_id observing_vertex_id x y z qx qy qz qw inf_11 inf_12 .. inf_16 inf_22 .. inf_66
+		int virtualVertexId = poses.size()?poses.rbegin()->first+1:0;
+		for(std::multimap<int, Link>::const_iterator iter = edgeConstraints.begin(); iter!=edgeConstraints.end(); ++iter)
+		{
+			std::string prefix = "EDGE_SE3:QUAT";
+			std::string suffix = "";
+
+			if(useRobustConstraints && iter->second.type() != Link::kNeighbor)
+			{
+				prefix = "EDGE_SE3_SWITCHABLE";
+				fprintf(file, "VERTEX_SWITCH %d 1\n", virtualVertexId);
+				fprintf(file, "EDGE_SWITCH_PRIOR %d 1 1.0\n", virtualVertexId);
+				suffix = uFormat(" %d", virtualVertexId++);
+			}
+
+			Eigen::Quaternionf q = iter->second.transform().getQuaternionf();
+			fprintf(file, "%s %d %d%s %f %f %f %f %f %f %f %f %f %f %f %f %f %f %f %f %f %f %f %f %f %f %f %f %f %f %f %f\n",
+					prefix.c_str(),
+					iter->first,
+					iter->second.to(),
+					suffix.c_str(),
+					iter->second.transform().x(),
+					iter->second.transform().y(),
+					iter->second.transform().z(),
+					q.x(),
+					q.y(),
+					q.z(),
+					q.w(),
+					iter->second.infMatrix().at<double>(0,0),
+					iter->second.infMatrix().at<double>(0,1),
+					iter->second.infMatrix().at<double>(0,2),
+					iter->second.infMatrix().at<double>(0,3),
+					iter->second.infMatrix().at<double>(0,4),
+					iter->second.infMatrix().at<double>(0,5),
+					iter->second.infMatrix().at<double>(1,1),
+					iter->second.infMatrix().at<double>(1,2),
+					iter->second.infMatrix().at<double>(1,3),
+					iter->second.infMatrix().at<double>(1,4),
+					iter->second.infMatrix().at<double>(1,5),
+					iter->second.infMatrix().at<double>(2,2),
+					iter->second.infMatrix().at<double>(2,3),
+					iter->second.infMatrix().at<double>(2,4),
+					iter->second.infMatrix().at<double>(2,5),
+					iter->second.infMatrix().at<double>(3,3),
+					iter->second.infMatrix().at<double>(3,4),
+					iter->second.infMatrix().at<double>(3,5),
+					iter->second.infMatrix().at<double>(4,4),
+					iter->second.infMatrix().at<double>(4,5),
+					iter->second.infMatrix().at<double>(5,5));
+		}
+		UINFO("Graph saved to %s", fileName.c_str());
+		fclose(file);
+	}
+	else
+	{
+		UERROR("Cannot save to file %s", fileName.c_str());
+		return false;
+	}
+	return true;
+}
+
+//////////////////////
+// GTSAM
+//////////////////////
+bool GTSAMOptimizer::available()
+{
+#ifdef WITH_GTSAM
+	return true;
+#else
+	return false;
+#endif
+}
+
+std::map<int, Transform> GTSAMOptimizer::optimize(
+		int rootId,
+		const std::map<int, Transform> & poses,
+		const std::multimap<int, Link> & edgeConstraints,
+		std::list<std::map<int, Transform> > * intermediateGraphes)
+{
+	std::map<int, Transform> optimizedPoses;
+#ifdef WITH_GTSAM
+	UDEBUG("Optimizing graph...");
+	if(edgeConstraints.size()>=1 && poses.size()>=2 && iterations() > 0)
+	{
+		gtsam::NonlinearFactorGraph graph;
+
+		//prior first pose
+		UASSERT(uContains(poses, rootId));
+		const Transform & initialPose = poses.at(rootId);
+		if(isSlam2d())
+		{
+			gtsam::noiseModel::Diagonal::shared_ptr priorNoise = gtsam::noiseModel::Diagonal::Sigmas(gtsam::Vector3(0.01, 0.01, 0.01));
+			graph.add(gtsam::PriorFactor<gtsam::Pose2>(rootId, gtsam::Pose2(initialPose.x(), initialPose.y(), initialPose.theta()), priorNoise));
+		}
+		else
+		{
+			gtsam::noiseModel::Diagonal::shared_ptr priorNoise = gtsam::noiseModel::Diagonal::Sigmas((gtsam::Vector(6) << 1e-6, 1e-6, 1e-6, 1e-4, 1e-4, 1e-4).finished());
+			graph.add(gtsam::PriorFactor<gtsam::Pose3>(rootId, gtsam::Pose3(initialPose.toEigen4d()), priorNoise));
+		}
+
+		UDEBUG("fill poses to gtsam...");
+		gtsam::Values initialEstimate;
+		for(std::map<int, Transform>::const_iterator iter = poses.begin(); iter!=poses.end(); ++iter)
+		{
+			UASSERT(!iter->second.isNull());
+			if(isSlam2d())
+			{
+				initialEstimate.insert(iter->first, gtsam::Pose2(iter->second.x(), iter->second.y(), iter->second.theta()));
+			}
+			else
+			{
+				initialEstimate.insert(iter->first, gtsam::Pose3(iter->second.toEigen4d()));
+			}
+		}
+
+		UDEBUG("fill edges to gtsam...");
+		int switchCounter = poses.rbegin()->first+1;
+		for(std::multimap<int, Link>::const_iterator iter=edgeConstraints.begin(); iter!=edgeConstraints.end(); ++iter)
+		{
+			int id1 = iter->first;
+			int id2 = iter->second.to();
+
+			UASSERT(!iter->second.transform().isNull());
+
+			if(this->isRobust() && iter->second.type()!=Link::kNeighbor)
+			{
+				// create new switch variable
+				// Sunderhauf IROS 2012:
+				// "Since it is reasonable to initially accept all loop closure constraints,
+				//  a proper and convenient initial value for all switch variables would be
+				//  sij = 1 when using the linear switch function"
+				double prior = 1.0;
+				initialEstimate.insert(gtsam::Symbol('s',switchCounter), vertigo::SwitchVariableLinear(prior));
+
+				// create switch prior factor
+				// "If the front-end is not able to assign sound individual values
+				//  for Ξij , it is save to set all Ξij = 1, since this value is close
+				//  to the individual optimal choice of Ξij for a large range of
+				//  outliers."
+				gtsam::noiseModel::Diagonal::shared_ptr switchPriorModel = gtsam::noiseModel::Diagonal::Sigmas(gtsam::Vector1(1.0));
+				graph.add(gtsam::PriorFactor<vertigo::SwitchVariableLinear> (gtsam::Symbol('s',switchCounter), vertigo::SwitchVariableLinear(prior), switchPriorModel));
+			}
+
+			if(isSlam2d())
+			{
+				Eigen::Matrix<double, 3, 3> information = Eigen::Matrix<double, 3, 3>::Identity();
+				if(!isCovarianceIgnored())
+				{
+					// For some reasons, dividing by 1000 avoids some exceptions (maybe too large numbers on optimization)
+					information(0,0) = iter->second.infMatrix().at<double>(0,0)/1000.0; // x-x
+					information(0,1) = iter->second.infMatrix().at<double>(0,1)/1000.0; // x-y
+					information(0,2) = iter->second.infMatrix().at<double>(0,5)/1000.0; // x-theta
+					information(1,0) = iter->second.infMatrix().at<double>(1,0)/1000.0; // y-x
+					information(1,1) = iter->second.infMatrix().at<double>(1,1)/1000.0; // y-y
+					information(1,2) = iter->second.infMatrix().at<double>(1,5)/1000.0; // y-theta
+					information(2,0) = iter->second.infMatrix().at<double>(5,0)/1000.0; // theta-x
+					information(2,1) = iter->second.infMatrix().at<double>(5,1)/1000.0; // theta-y
+					information(2,2) = iter->second.infMatrix().at<double>(5,5)/1000.0; // theta-theta
+				}
+				gtsam::noiseModel::Gaussian::shared_ptr model = gtsam::noiseModel::Gaussian::Information(information);
+
+				if(this->isRobust() && iter->second.type()!=Link::kNeighbor)
+				{
+					// create switchable edge factor
+					graph.add(vertigo::BetweenFactorSwitchableLinear<gtsam::Pose2>(id1, id2, gtsam::Symbol('s', switchCounter++), gtsam::Pose2(iter->second.transform().x(), iter->second.transform().y(), iter->second.transform().theta()), model));
+				}
+				else
+				{
+					graph.add(gtsam::BetweenFactor<gtsam::Pose2>(id1, id2, gtsam::Pose2(iter->second.transform().x(), iter->second.transform().y(), iter->second.transform().theta()), model));
+				}
+			}
+			else
+			{
+				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));
+					// For some reasons, dividing by 1000 avoids some exceptions (maybe too large numbers on optimization)
+					information = information / 1000.0;
+				}
+
+				gtsam::noiseModel::Gaussian::shared_ptr model = gtsam::noiseModel::Gaussian::Information(information);
+
+				if(this->isRobust() && iter->second.type()!=Link::kNeighbor)
+				{
+					// create switchable edge factor
+					graph.add(vertigo::BetweenFactorSwitchableLinear<gtsam::Pose3>(id1, id2, gtsam::Symbol('s', switchCounter++), gtsam::Pose3(iter->second.transform().toEigen4d()), model));
+				}
+				else
+				{
+					graph.add(gtsam::BetweenFactor<gtsam::Pose3>(id1, id2, gtsam::Pose3(iter->second.transform().toEigen4d()), model));
+				}
+			}
+		}
+
+		UDEBUG("create optimizer");
+		gtsam::GaussNewtonParams parameters;
+		parameters.relativeErrorTol = epsilon();
+		parameters.maxIterations = iterations();
+		gtsam::GaussNewtonOptimizer optimizer(graph, initialEstimate, parameters);
+		//gtsam::LevenbergMarquardtParams parametersLev;
+		//parametersLev.relativeErrorTol = epsilon();
+		//parametersLev.maxIterations = iterations();
+		//gtsam::LevenbergMarquardtOptimizer optimizer(graph, initialEstimate, parametersLev);
+		//gtsam::DoglegParams parametersDogleg;
+		//parametersDogleg.relativeErrorTol = epsilon();
+		//parametersDogleg.maxIterations = iterations();
+		//gtsam::DoglegOptimizer optimizer(graph, initialEstimate, parametersDogleg);
+
+		UINFO("GTSAM optimizing begin (max iterations=%d, robust=%d)", iterations(), isRobust()?1:0);
+		UTimer timer;
+		for(int i=0; i<iterations(); ++i)
+		{
+			if(intermediateGraphes && i > 0)
+			{
+				std::map<int, Transform> tmpPoses;
+				for(gtsam::Values::const_iterator iter=optimizer.values().begin(); iter!=optimizer.values().end(); ++iter)
+				{
+					if(iter->value.dim() > 1)
+					{
+						if(isSlam2d())
+						{
+							gtsam::Pose2 p = iter->value.cast<gtsam::Pose2>();
+							tmpPoses.insert(std::make_pair((int)iter->key, Transform(p.x(), p.y(), p.theta())));
+						}
+						else
+						{
+							gtsam::Pose3 p = iter->value.cast<gtsam::Pose3>();
+							tmpPoses.insert(std::make_pair((int)iter->key, Transform::fromEigen4d(p.matrix())));
+						}
+					}
+				}
+				intermediateGraphes->push_back(tmpPoses);
+			}
+			try
+			{
+				optimizer.iterate();
+			}
+			catch(gtsam::IndeterminantLinearSystemException & e)
+			{
+				UERROR("GTSAM exception catched: %s", e.what());
+				return optimizedPoses;
+			}
+			UDEBUG("iteration %d error =%f", i+1, optimizer.error());
+			if(optimizer.error() < epsilon())
+			{
+				break;
+			}
+		}
+		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());
+
+		for(gtsam::Values::const_iterator iter=optimizer.values().begin(); iter!=optimizer.values().end(); ++iter)
+		{
+			if(iter->value.dim() > 1)
+			{
+				if(isSlam2d())
+				{
+					gtsam::Pose2 p = iter->value.cast<gtsam::Pose2>();
+					optimizedPoses.insert(std::make_pair((int)iter->key, Transform(p.x(), p.y(), p.theta())));
+				}
+				else
+				{
+					gtsam::Pose3 p = iter->value.cast<gtsam::Pose3>();
+					optimizedPoses.insert(std::make_pair((int)iter->key, Transform::fromEigen4d(p.matrix())));
+				}
+			}
+		}
+	}
+	UDEBUG("Optimizing graph...end!");
+#else
+	UERROR("Not built with GTSAM support!");
+#endif
+	return optimizedPoses;
+}
+
 //////////////////////
 // cvsba
 //////////////////////

corelib/src/Rtabmap.cpp

@@ -742,6 +742,14 @@ void Rtabmap::exportPoses(const std::string & path, bool optimized, bool global,
 		{
 			graph::TOROOptimizer::saveGraph(path, poses, constraints);
 		}
+		else if(type == 4) // g2o
+		{
+#ifdef WITH_G2O
+			graph::G2OOptimizer::saveGraph(path, poses, constraints);
+#else
+			UERROR("Cannot export in g2o format because RTAB-Map is not built with g2o support!");
+#endif
+		}
 		else
 		{
 			//get timestamps
@@ -2680,7 +2688,6 @@ void Rtabmap::optimizeCurrentMap(
 		std::multimap<int, Link> * constraints) const
 {
 	//Optimize the map
-	optimizedPoses.clear();
 	UINFO("Optimize map: around location %d", id);
 	if(_memory && id > 0)
 	{
@@ -2692,14 +2699,23 @@ void Rtabmap::optimizeCurrentMap(
 		}
 		UINFO("get %d ids time %f s", (int)ids.size(), timer.ticks());
 
-		optimizedPoses = Rtabmap::optimizeGraph(id, uKeysSet(ids), lookInDatabase, constraints);
-
-		if(_memory->getSignature(id) && uContains(optimizedPoses, id))
-		{
-			Transform t = optimizedPoses.at(id) * _memory->getSignature(id)->getPose().inverse();
-			UINFO("Correction (from node %d) %s", id, t.prettyPrint().c_str());
-		}
+		std::map<int, Transform> poses = Rtabmap::optimizeGraph(id, uKeysSet(ids), lookInDatabase, constraints);
 		UINFO("optimize time %f s", timer.ticks());
+
+		if(poses.size())
+		{
+			optimizedPoses = poses;
+
+			if(_memory->getSignature(id) && uContains(optimizedPoses, id))
+			{
+				Transform t = optimizedPoses.at(id) * _memory->getSignature(id)->getPose().inverse();
+				UINFO("Correction (from node %d) %s", id, t.prettyPrint().c_str());
+			}
+		}
+		else
+		{
+			UERROR("Failed to optimize the graph! Keeping the graph without optimization...");
+		}
 	}
 }
 

corelib/src/Transform.cpp

@@ -66,6 +66,12 @@ Transform::Transform(float x, float y, float z, float roll, float pitch, float y
 	*this = fromEigen3f(t);
 }
 
+Transform::Transform(float x, float y, float theta)
+{
+	Eigen::Affine3f t = pcl::getTransformation (x, y, 0, 0, 0, theta);
+	*this = fromEigen3f(t);
+}
+
 bool Transform::isNull() const
 {
 	return (data()[0] == 0.0f &&

corelib/src/sqlite3/readme.txt

@@ -0,0 +1,3 @@
+
+Info: https://www.sqlite.org/
+License: Public domain (https://www.sqlite.org/copyright.html)

corelib/src/toro3d/readme.txt

@@ -0,0 +1,3 @@
+
+Info: https://www.openslam.org/toro.html
+License: Creative Commons (Attribution-NonCommercial-ShareAlike)

corelib/src/vertigo/g2o/edge_se2MaxMixture.cpp

@@ -0,0 +1,122 @@
+/*
+ * edge_se2MaxMixture.cpp
+ *
+ *  Created on: 12.06.2012
+ *      Author: niko
+ */
+
+
+#include "edge_se2MaxMixture.h"
+
+#include <GL/gl.h>
+using namespace std;
+using namespace Eigen;
+
+// ================================================
+EdgeSE2MaxMixture::EdgeSE2MaxMixture() : g2o::EdgeSE2::EdgeSE2()
+{
+  nullHypothesisMoreLikely = false;
+}
+
+// ================================================
+bool EdgeSE2MaxMixture::read(std::istream& is)
+  {
+    Vector3d p;
+    is >>  weight >> p[0] >> p[1] >> p[2];
+    setMeasurement(g2o::SE2(p));
+    _inverseMeasurement = measurement().inverse();
+    //measurement().fromVector(p);
+    //inverseMeasurement() = measurement().inverse();
+    for (int i = 0; i < 3; ++i)
+      for (int j = i; j < 3; ++j) {
+        is >> information()(i, j);
+        if (i != j)
+          information()(j, i) = information()(i, j);
+      }
+
+    information_constraint = _information;
+    nu_constraint = 1.0/sqrt(information_constraint.inverse().determinant());
+    information_nullHypothesis = information_constraint*weight;
+    nu_nullHypothesis = 1.0/sqrt(information_nullHypothesis.inverse().determinant());
+
+    return true;
+  }
+// ================================================
+bool EdgeSE2MaxMixture::write(std::ostream& os) const
+{
+    Vector3d p = measurement().toVector();
+    os << p.x() << " " << p.y() << " " << p.z();
+    for (int i = 0; i < 3; ++i)
+      for (int j = i; j < 3; ++j)
+        os << " " << information()(i, j);
+    return os.good();
+}
+
+// ================================================
+void EdgeSE2MaxMixture::linearizeOplus()
+{
+  g2o::EdgeSE2::linearizeOplus();
+  if (nullHypothesisMoreLikely) {
+    _jacobianOplusXi *= weight;
+    _jacobianOplusXj *= weight;
+  }
+}
+
+// ================================================
+void EdgeSE2MaxMixture::computeError()
+{
+  // calculate the error for this constraint
+  g2o::EdgeSE2::computeError();
+
+  // determine the likelihood for constraint and null hypothesis
+  double mahal_constraint = _error.transpose() * information_constraint * _error;
+  double likelihood_constraint = nu_constraint * exp(-mahal_constraint);
+
+  double mahal_nullHypothesis  = _error.transpose() * (information_nullHypothesis) * _error;
+  double likelihood_nullHypothesis = nu_nullHypothesis * exp(-mahal_nullHypothesis);
+
+  // if the nullHypothesis is more likely ...
+  if (likelihood_nullHypothesis > likelihood_constraint) {
+    _information = information_nullHypothesis;
+    nullHypothesisMoreLikely = true;
+  }
+  else {
+    _information = information_constraint;
+    nullHypothesisMoreLikely = false;
+  }
+
+
+}
+
+
+// ================================================
+#ifdef G2O_HAVE_OPENGL
+  EdgeSE2MaxMixtureDrawAction::EdgeSE2MaxMixtureDrawAction(): DrawAction(typeid(EdgeSE2MaxMixture).name()){}
+
+  g2o::HyperGraphElementAction* EdgeSE2MaxMixtureDrawAction::operator()(g2o::HyperGraph::HyperGraphElement* element,
+               g2o::HyperGraphElementAction::Parameters* /*params_*/){
+    if (typeid(*element).name()!=_typeName)
+      return 0;
+    EdgeSE2MaxMixture* e =  static_cast<EdgeSE2MaxMixture*>(element);
+
+
+    g2o::VertexSE2* fromEdge = static_cast<g2o::VertexSE2*>(e->vertices()[0]);
+    g2o::VertexSE2* toEdge   = static_cast<g2o::VertexSE2*>(e->vertices()[1]);
+
+
+    if (e->nullHypothesisMoreLikely) glColor3f(0.0,0.0,0.0);
+    else glColor3f(1.0,0.5,0.2);
+
+    glPushAttrib(GL_ENABLE_BIT);
+    glDisable(GL_LIGHTING);
+    glBegin(GL_LINES);
+    glVertex3f(fromEdge->estimate().translation().x(),fromEdge->estimate().translation().y(),0.);
+    glVertex3f(toEdge->estimate().translation().x(),toEdge->estimate().translation().y(),0.);
+    glEnd();
+    glPopAttrib();
+    return this;
+  }
+#endif
+
+
+

corelib/src/vertigo/g2o/edge_se2MaxMixture.h

@@ -0,0 +1,46 @@
+/*
+ * edge_se2MaxMixture.h
+ *
+ *  Created on: 12.06.2012
+ *      Author: niko
+ */
+
+#ifndef EDGE_SE2MAXMIXTURE_H_
+#define EDGE_SE2MAXMIXTURE_H_
+
+#include "g2o/types/slam2d/vertex_se2.h"
+#include "g2o/types/slam2d/edge_se2.h"
+
+
+class EdgeSE2MaxMixture : public g2o::EdgeSE2
+{
+  public:
+    EdgeSE2MaxMixture();
+
+    virtual bool read(std::istream& is);
+    virtual bool write(std::ostream& os) const;
+    void computeError();
+    void linearizeOplus();
+
+    double weight;
+    
+    bool nullHypothesisMoreLikely;
+    
+    InformationType information_nullHypothesis;
+    double nu_nullHypothesis;
+    InformationType information_constraint;
+    double nu_constraint ;
+};
+
+
+#ifdef G2O_HAVE_OPENGL
+    class EdgeSE2MaxMixtureDrawAction: public g2o::DrawAction{
+    public:
+      EdgeSE2MaxMixtureDrawAction();
+      virtual g2o::HyperGraphElementAction* operator()(g2o::HyperGraph::HyperGraphElement* element,
+              g2o::HyperGraphElementAction::Parameters* params_);
+    };
+#endif
+
+
+#endif /* EDGE_SE2MAXMIXTURE_H_ */

corelib/src/vertigo/g2o/edge_se2Switchable.cpp

@@ -0,0 +1,131 @@
+/*
+ * edge_se2Switchable.cpp
+ *
+ *  Created on: 13.07.2011
+ *      Author: niko
+ *
+ *  Updated on: 14.01.2013
+ *      Author: Christian Kerl <christian.kerl@in.tum.de>
+ */
+
+#include "vertigo/g2o/edge_se2Switchable.h"
+#include "vertigo/g2o/vertex_switchLinear.h"
+#include <GL/gl.h>
+
+using namespace std;
+using namespace Eigen;
+
+
+// ================================================
+EdgeSE2Switchable::EdgeSE2Switchable() : g2o::BaseMultiEdge<3, g2o::SE2>()
+{
+  resize(3);
+  _jacobianOplus[0].resize(3,3);    
+  _jacobianOplus[1].resize(3,3);
+  _jacobianOplus[2].resize(3,1); 
+
+}
+// ================================================
+bool EdgeSE2Switchable::read(std::istream& is)
+  {
+    Vector3d p;
+    is >> p[0] >> p[1] >> p[2];
+    setMeasurement(g2o::SE2(p));
+    _inverseMeasurement = measurement().inverse();
+
+    for (int i = 0; i < 3; ++i)
+      for (int j = i; j < 3; ++j) {
+        is >> information()(i, j);
+        if (i != j)
+          information()(j, i) = information()(i, j);
+      }
+    return true;
+  }
+// ================================================
+bool EdgeSE2Switchable::write(std::ostream& os) const
+{
+    Vector3d p = measurement().toVector();
+    os << p.x() << " " << p.y() << " " << p.z();
+    for (int i = 0; i < 3; ++i)
+      for (int j = i; j < 3; ++j)
+        os << " " << information()(i, j);
+    return os.good();
+}
+
+// ================================================
+void EdgeSE2Switchable::linearizeOplus()
+{
+   
+    const g2o::VertexSE2* vi = static_cast<const g2o::VertexSE2*>(_vertices[0]);
+    const g2o::VertexSE2* vj = static_cast<const g2o::VertexSE2*>(_vertices[1]);
+    const VertexSwitchLinear* vSwitch = static_cast<const VertexSwitchLinear*>(_vertices[2]);
+
+    double thetai = vi->estimate().rotation().angle();
+
+    Vector2d dt = vj->estimate().translation() - vi->estimate().translation();
+    double si=sin(thetai), ci=cos(thetai);
+
+    _jacobianOplus[0](0, 0) = -ci; _jacobianOplus[0](0, 1) = -si; _jacobianOplus[0](0, 2) = -si*dt.x()+ci*dt.y();
+    _jacobianOplus[0](1, 0) =  si; _jacobianOplus[0](1, 1) = -ci; _jacobianOplus[0](1, 2) = -ci*dt.x()-si*dt.y();
+    _jacobianOplus[0](2, 0) =  0;  _jacobianOplus[0](2, 1) = 0;   _jacobianOplus[0](2, 2) = -1;
+
+    _jacobianOplus[1](0, 0) = ci; _jacobianOplus[1](0, 1)= si; _jacobianOplus[1](0, 2)= 0;
+    _jacobianOplus[1](1, 0) =-si; _jacobianOplus[1](1, 1)= ci; _jacobianOplus[1](1, 2)= 0;
+    _jacobianOplus[1](2, 0) = 0;  _jacobianOplus[1](2, 1)= 0;  _jacobianOplus[1](2, 2)= 1;
+
+    const g2o::SE2& rmean = _inverseMeasurement;
+    Matrix3d z = Matrix3d::Zero();
+    z.block<2, 2>(0, 0) = rmean.rotation().toRotationMatrix();
+    z(2, 2) = 1.;
+    _jacobianOplus[0] = z * _jacobianOplus[0];
+    _jacobianOplus[1] = z * _jacobianOplus[1];
+
+
+    _jacobianOplus[0]*=vSwitch->estimate();
+    _jacobianOplus[1]*=vSwitch->estimate();
+
+
+    // derivative w.r.t switch vertex
+    _jacobianOplus[2].setZero();
+    g2o::SE2 delta = _inverseMeasurement * (vi->estimate().inverse()*vj->estimate());
+    _jacobianOplus[2] = delta.toVector() * vSwitch->gradient();  
+}
+
+// ================================================
+void EdgeSE2Switchable::computeError()
+{
+    const g2o::VertexSE2* v1 = static_cast<const g2o::VertexSE2*>(_vertices[0]);
+    const g2o::VertexSE2* v2 = static_cast<const g2o::VertexSE2*>(_vertices[1]);
+    const VertexSwitchLinear* v3 = static_cast<const VertexSwitchLinear*>(_vertices[2]);
+  
+    g2o::SE2 delta = _inverseMeasurement * (v1->estimate().inverse()*v2->estimate());
+    _error = delta.toVector() * v3->estimate();
+}
+
+
+#ifdef G2O_HAVE_OPENGL
+  EdgeSE2SwitchableDrawAction::EdgeSE2SwitchableDrawAction(): DrawAction(typeid(EdgeSE2Switchable).name()){}
+
+  g2o::HyperGraphElementAction* EdgeSE2SwitchableDrawAction::operator()(g2o::HyperGraph::HyperGraphElement* element,
+               g2o::HyperGraphElementAction::Parameters* /*params_*/){
+    if (typeid(*element).name()!=_typeName)
+      return 0;
+    EdgeSE2Switchable* e =  static_cast<EdgeSE2Switchable*>(element);
+
+
+    g2o::VertexSE2* fromEdge = static_cast<g2o::VertexSE2*>(e->vertices()[0]);
+    g2o::VertexSE2* toEdge   = static_cast<g2o::VertexSE2*>(e->vertices()[1]);
+    VertexSwitchLinear* s   = static_cast<VertexSwitchLinear*>(e->vertices()[2]);
+
+    glColor3f(s->estimate()*1.0,s->estimate()*0.1,s->estimate()*0.1);
+    glPushAttrib(GL_ENABLE_BIT);
+    glDisable(GL_LIGHTING);
+    glBegin(GL_LINES);
+    glVertex3f(fromEdge->estimate().translation().x(),fromEdge->estimate().translation().y(),0.);
+    glVertex3f(toEdge->estimate().translation().x(),toEdge->estimate().translation().y(),0.);
+    glEnd();
+    glPopAttrib();
+    return this;
+  }
+#endif
+

corelib/src/vertigo/g2o/edge_se2Switchable.h

@@ -0,0 +1,50 @@
+/*
+ * edge_se2Switchable.h
+ *
+ *  Created on: 13.07.2011
+ *      Author: niko
+ *
+ *  Updated on: 14.01.2013
+ *      Author: Christian Kerl <christian.kerl@in.tum.de>
+ */
+
+#ifndef EDGE_SE2SWITCHABLE_H_
+#define EDGE_SE2SWITCHABLE_H_
+
+#include "g2o/types/slam2d/vertex_se2.h"
+#include "g2o/core/base_multi_edge.h"
+#include "g2o/core/hyper_graph_action.h"
+
+class EdgeSE2Switchable : public g2o::BaseMultiEdge<3, g2o::SE2>
+{
+  public:
+    EdgeSE2Switchable();
+
+    virtual bool read(std::istream& is);
+    virtual bool write(std::ostream& os) const;
+    void computeError();
+    void linearizeOplus();
+
+
+    virtual void setMeasurement(const g2o::SE2& m){
+      _measurement = m;
+      _inverseMeasurement = m.inverse();
+    }
+
+    protected:
+      g2o::SE2 _inverseMeasurement;
+};
+
+
+#ifdef G2O_HAVE_OPENGL
+    class EdgeSE2SwitchableDrawAction: public g2o::DrawAction{
+    public:
+      EdgeSE2SwitchableDrawAction();
+      virtual g2o::HyperGraphElementAction* operator()(g2o::HyperGraph::HyperGraphElement* element,
+              g2o::HyperGraphElementAction::Parameters* params_);
+    };
+#endif
+
+
+
+#endif /* EDGE_SE2SWITCHABLE_H_ */

corelib/src/vertigo/g2o/edge_se3Switchable.cpp

@@ -0,0 +1,120 @@
+/*
+ * edge_se3Switchable.cpp
+ *
+ *  Created on: 17.10.2011
+ *      Author: niko
+ *
+ *  Updated on: 14.01.2013
+ *      Author: Christian Kerl <christian.kerl@in.tum.de>
+ */
+
+#include "vertigo/g2o/edge_se3Switchable.h"
+#include "vertigo/g2o/vertex_switchLinear.h"
+#include <GL/gl.h>
+#include "g2o/types/slam3d/vertex_se3.h"
+#include "g2o/types/slam3d/isometry3d_gradients.h"
+
+
+using namespace std;
+using namespace Eigen;
+
+
+// ================================================
+EdgeSE3Switchable::EdgeSE3Switchable() : g2o::BaseMultiEdge<6, Eigen::Isometry3d>()
+{
+  resize(3);
+  _jacobianOplus[0].resize(6,6); 
+  _jacobianOplus[1].resize(6,6);
+  _jacobianOplus[2].resize(6,1);
+
+}
+// ================================================
+bool EdgeSE3Switchable::read(std::istream& is)
+  {    
+    g2o::Vector7d meas;
+    for (int i=0; i<7; i++) 
+      is >> meas[i];
+    // normalize the quaternion to recover numerical precision lost by storing as human readable text
+    Vector4d::MapType(meas.data()+3).normalize();
+    setMeasurement(g2o::internal::fromVectorQT(meas));
+
+    for (int i=0; i<6; i++)
+      for (int j=i; j<6; j++) {
+        is >> information()(i,j);
+        if (i!=j)
+          information()(j,i) = information()(i,j);
+      }
+    return true;
+
+  }
+// ================================================
+bool EdgeSE3Switchable::write(std::ostream& os) const
+{
+    g2o::Vector7d meas = g2o::internal::toVectorQT(measurement());
+    for (int i=0; i<7; i++) os  << meas[i] << " ";
+    for (int i = 0; i < 6; ++i)
+      for (int j = i; j < 6; ++j)
+        os << " " << information()(i, j);
+    return os.good();
+}
+
+// ================================================
+void EdgeSE3Switchable::linearizeOplus()
+{
+
+    g2o::VertexSE3* from = static_cast<g2o::VertexSE3*>(_vertices[0]);
+    g2o::VertexSE3* to = static_cast<g2o::VertexSE3*>(_vertices[1]);
+    const VertexSwitchLinear* vSwitch = static_cast<const VertexSwitchLinear*>(_vertices[2]);
+
+    Eigen::Isometry3d E;
+    const Eigen::Isometry3d& Xi=from->estimate();
+    const Eigen::Isometry3d& Xj=to->estimate();
+    const Eigen::Isometry3d& Z=_measurement;
+    g2o::internal::computeEdgeSE3Gradient(E, _jacobianOplus[0], _jacobianOplus[1], Z, Xi, Xj);
+
+    _jacobianOplus[0]*=vSwitch->estimate();
+    _jacobianOplus[1]*=vSwitch->estimate();
+
+    // derivative w.r.t switch vertex
+    _jacobianOplus[2].setZero();
+    _jacobianOplus[2] = g2o::internal::toVectorMQT(E) * vSwitch->gradient();
+}
+
+
+// ================================================
+void EdgeSE3Switchable::computeError()
+{
+    const g2o::VertexSE3* v1 = dynamic_cast<const g2o::VertexSE3*>(_vertices[0]);
+    const g2o::VertexSE3* v2 = dynamic_cast<const g2o::VertexSE3*>(_vertices[1]);
+    const VertexSwitchLinear* v3 = static_cast<const VertexSwitchLinear*>(_vertices[2]);
+
+    Eigen::Isometry3d delta = _inverseMeasurement * (v1->estimate().inverse()*v2->estimate());
+    _error = g2o::internal::toVectorMQT(delta) * v3->estimate();
+}
+
+
+#ifdef G2O_HAVE_OPENGL
+  EdgeSE3SwitchableDrawAction::EdgeSE3SwitchableDrawAction(): DrawAction(typeid(EdgeSE3Switchable).name()){}
+
+  g2o::HyperGraphElementAction* EdgeSE3SwitchableDrawAction::operator()(g2o::HyperGraph::HyperGraphElement* element,
+               g2o::HyperGraphElementAction::Parameters* /*params_*/){
+    if (typeid(*element).name()!=_typeName)
+      return 0;
+    EdgeSE3Switchable* e =  static_cast<EdgeSE3Switchable*>(element);
+
+
+    g2o::VertexSE3* fromEdge = static_cast<g2o::VertexSE3*>(e->vertices()[0]);
+    g2o::VertexSE3* toEdge   = static_cast<g2o::VertexSE3*>(e->vertices()[1]);
+    VertexSwitchLinear* s   = static_cast<VertexSwitchLinear*>(e->vertices()[2]);
+
+    glColor3f(s->estimate()*1.0,s->estimate()*0.1,s->estimate()*0.1);
+    glPushAttrib(GL_ENABLE_BIT);
+    glDisable(GL_LIGHTING);
+    glBegin(GL_LINES);
+    glVertex3f(fromEdge->estimate().translation().x(),fromEdge->estimate().translation().y(),fromEdge->estimate().translation().z());
+    glVertex3f(toEdge->estimate().translation().x(),toEdge->estimate().translation().y(),toEdge->estimate().translation().z());
+    glEnd();
+    glPopAttrib();
+    return this;
+  }
+#endif

corelib/src/vertigo/g2o/edge_se3Switchable.h

@@ -0,0 +1,48 @@
+/*
+ * edge_se3Switchable.h
+ *
+ *  Created on: 17.10.2011
+ *      Author: niko
+ *
+ *  Updated on: 14.01.2013
+ *      Author: Christian Kerl <christian.kerl@in.tum.de>
+ */
+
+#ifndef EDGE_SE3SWITCHABLE_H_
+#define EDGE_SE3SWITCHABLE_H_
+
+#include "g2o/types/slam3d/vertex_se3.h"
+#include "g2o/core/base_multi_edge.h"
+#include "g2o/core/hyper_graph_action.h"
+
+class EdgeSE3Switchable : public g2o::BaseMultiEdge<6, Eigen::Isometry3d>
+{
+  public:
+    EdgeSE3Switchable();
+
+    virtual bool read(std::istream& is);
+    virtual bool write(std::ostream& os) const;
+    void computeError();
+    void linearizeOplus();
+
+    virtual void setMeasurement(const Eigen::Isometry3d& m){
+      _measurement = m;
+      _inverseMeasurement = m.inverse();
+    }
+
+  protected:
+    Eigen::Isometry3d _inverseMeasurement;
+};
+
+
+#ifdef G2O_HAVE_OPENGL
+    class EdgeSE3SwitchableDrawAction: public g2o::DrawAction{
+    public:
+      EdgeSE3SwitchableDrawAction();
+      virtual g2o::HyperGraphElementAction* operator()(g2o::HyperGraph::HyperGraphElement* element,
+              g2o::HyperGraphElementAction::Parameters* params_);
+    };
+#endif
+
+
+#endif /* EDGE_SE3SWITCHABLE_H_ */

corelib/src/vertigo/g2o/edge_switchPrior.cpp

@@ -0,0 +1,44 @@
+#include "vertigo/g2o/edge_switchPrior.h"
+using namespace std;
+
+    EdgeSwitchPrior::EdgeSwitchPrior()
+    {
+    	setMeasurement(1.0);
+    }
+
+    bool EdgeSwitchPrior::read(std::istream &is)
+    {
+      double new_measurement;
+      is >> new_measurement;
+
+      setMeasurement(new_measurement);
+
+      is >> information()(0,0);
+      return true;
+    }
+
+    bool EdgeSwitchPrior::write(std::ostream &os) const
+    {
+      os << measurement() << " " << information()(0,0);
+      return true;
+    }
+
+    void EdgeSwitchPrior::setMeasurement(const double & m)
+    {
+    	g2o::BaseEdge<1, double>::setMeasurement(m);
+    	information()(0,0) = 1.0;
+    }
+
+    void EdgeSwitchPrior::linearizeOplus()
+    {
+      _jacobianOplusXi[0]=-1.0;
+    }
+
+    void EdgeSwitchPrior::computeError()
+    {
+      const VertexSwitchLinear* s = static_cast<const VertexSwitchLinear*>(_vertices[0]);
+
+      _error[0] = measurement() - s->x();
+
+    }
+

corelib/src/vertigo/g2o/edge_switchPrior.h

@@ -0,0 +1,22 @@
+#pragma once
+
+
+#include "vertex_switchLinear.h"
+#include "g2o/core/base_unary_edge.h"
+
+
+
+class EdgeSwitchPrior : public g2o::BaseUnaryEdge<1, double, VertexSwitchLinear>
+{
+  public:
+    EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+    EdgeSwitchPrior();
+
+    virtual bool read(std::istream& is);
+    virtual bool write(std::ostream& os) const;
+
+    virtual void setMeasurement(const double & m);
+
+    virtual void linearizeOplus();
+    void computeError();
+};

corelib/src/vertigo/g2o/types_g2o_robust.cpp

@@ -0,0 +1,22 @@
+#include "g2o/core/factory.h"
+#include "g2o/stuff/macros.h"
+
+#include "vertigo/g2o/edge_switchPrior.h"
+#include "vertigo/g2o/edge_se2Switchable.h"
+//#include "vertigo/g2o/edge_se2MaxMixture.h"
+#include "vertigo/g2o/edge_se3Switchable.h"
+#include "vertigo/g2o/vertex_switchLinear.h"
+
+
+G2O_REGISTER_TYPE(EDGE_SWITCH_PRIOR, EdgeSwitchPrior);
+G2O_REGISTER_TYPE(EDGE_SE2_SWITCHABLE, EdgeSE2Switchable);
+//G2O_REGISTER_TYPE(EDGE_SE2_MAXMIX, EdgeSE2MaxMixture);
+G2O_REGISTER_TYPE(EDGE_SE3_SWITCHABLE, EdgeSE3Switchable);
+G2O_REGISTER_TYPE(VERTEX_SWITCH, VertexSwitchLinear);
+
+#ifdef G2O_HAVE_OPENGL
+  G2O_REGISTER_ACTION(EdgeSE2SwitchableDrawAction);
+  //G2O_REGISTER_ACTION(EdgeSE2MaxMixtureDrawAction);
+  G2O_REGISTER_ACTION(EdgeSE3SwitchableDrawAction);
+#endif
+

corelib/src/vertigo/g2o/vertex_switchLinear.cpp

@@ -0,0 +1,59 @@
+/*
+ * vertex_switchLinear.cpp
+ *
+ *  Created on: 17.10.2011
+ *      Author: niko
+ *
+ *  Updated on: 14.01.2013
+ *      Author: Christian Kerl <christian.kerl@in.tum.de>
+ */
+
+#include "vertigo/g2o/vertex_switchLinear.h"
+#include <iostream>
+
+using namespace std;
+
+    VertexSwitchLinear::VertexSwitchLinear() :
+		_x(0)
+    {
+    	setToOrigin();
+    	setEstimate(1.0);
+    }
+
+    bool VertexSwitchLinear:: read(std::istream& is)
+    {
+      is >> _x;
+      _estimate=_x;
+
+      return true;
+    }
+
+    bool  VertexSwitchLinear::write(std::ostream& os) const
+    {
+      os << _x;
+      return os.good();
+    }
+
+    void VertexSwitchLinear::setToOriginImpl()
+    {
+      _x=0;
+      _estimate=_x;
+    }
+
+
+    void VertexSwitchLinear::setEstimate(const double &et)
+    {
+      _x=et;
+      _estimate=_x;
+    }
+
+
+    void VertexSwitchLinear::oplusImpl(const double* update)
+    {
+      _x += update[0];
+
+      if (_x<0) _x=0;
+      if (_x>1) _x=1;
+
+      _estimate=_x;
+    }

corelib/src/vertigo/g2o/vertex_switchLinear.h

@@ -0,0 +1,43 @@
+/*
+ * vertex_switchLinear.h
+ *
+ *  Created on: 17.10.2011
+ *      Author: niko
+ *
+ *  Updated on: 14.01.2013
+ *      Author: Christian Kerl <christian.kerl@in.tum.de>
+ */
+
+
+#pragma once
+
+#include "g2o/core/base_vertex.h"
+#include <math.h>
+
+
+
+    class VertexSwitchLinear : public g2o::BaseVertex<1, double>
+    {
+
+    public:
+      VertexSwitchLinear();
+
+      virtual void setToOriginImpl();
+
+      virtual void oplusImpl(const double* update);
+
+      virtual bool read(std::istream& is);
+      virtual bool write(std::ostream& os) const;
+      virtual void setEstimate(const double &et);
+
+
+      double x() const { return _x; };
+
+
+      //! The gradient at the current estimate is always 1;
+      double gradient() const { return 1; } ;
+
+    private:
+      double _x;
+
+    };

corelib/src/vertigo/gtsam/betweenFactorMaxMix.h

@@ -0,0 +1,67 @@
+/*
+ * betweenFactorMaxMix.h
+ *
+ *  Created on: 14.08.2012
+ *      Author: niko
+ */
+
+#ifndef BETWEENFACTORMAXMIX_H_
+#define BETWEENFACTORMAXMIX_H_
+
+#include <gtsam/linear/NoiseModel.h>
+#include <Eigen/Eigen>
+
+namespace vertigo {
+
+  template<class VALUE>
+  class BetweenFactorMaxMix : public gtsam::NoiseModelFactor2<VALUE, VALUE>
+  {
+    public:
+      BetweenFactorMaxMix() : weight(0.0) {};
+      BetweenFactorMaxMix(gtsam::Key key1, gtsam::Key key2, const VALUE& measured, const gtsam::SharedNoiseModel& model, const gtsam::SharedNoiseModel& model2, double w)
+      : gtsam::NoiseModelFactor2<VALUE, VALUE>(model, key1, key2), weight(w), nullHypothesisModel(model2),
+        betweenFactor(key1, key2, measured, model)  {   };
+
+      gtsam::Vector evaluateError(const VALUE& p1, const VALUE& p2,
+          boost::optional<gtsam::Matrix&> H1 = boost::none,
+          boost::optional<gtsam::Matrix&> H2 =  boost::none) const
+        {
+
+          // calculate error
+          gtsam::Vector error = betweenFactor.evaluateError(p1, p2, H1, H2);
+
+
+
+          // which hypothesis is more likely
+          double m1 = this->noiseModel_->distance(error);
+          gtsam::noiseModel::Gaussian::shared_ptr g1 = this->noiseModel_;
+          gtsam::Matrix info1(g1->R().transpose()*g1->R());
+          double nu1 = 1.0/sqrt(gtsam::inverse(info1).determinant());
+          double l1 = nu1 * exp(-0.5*m1);
+
+          double m2 = nullHypothesisModel->distance(error);
+          gtsam::noiseModel::Gaussian::shared_ptr g2 = nullHypothesisModel;
+          gtsam::Matrix info2(g2->R().transpose()*g2->R());
+          double nu2 = 1.0/sqrt(gtsam::inverse(info2).determinant());
+          double l2 = nu2 * exp(-0.5*m2);
+
+          // if the null hypothesis is more likely, than proceed by applying the weight ...
+          if (l2>l1) {
+            if (H1) *H1 = *H1 * weight;
+            if (H2) *H2 = *H2 * weight;
+            error *= sqrt(weight);
+          }
+
+          return error;
+        };
+
+    private:
+      gtsam::BetweenFactor<VALUE> betweenFactor;
+      gtsam::SharedNoiseModel nullHypothesisModel;
+      double weight;
+
+  };
+}
+
+
+#endif /* BETWEENFACTORMAXMIX_H_ */

corelib/src/vertigo/gtsam/betweenFactorSwitchable.h

@@ -0,0 +1,97 @@
+/*
+ * betweenFactorSwitchable.h
+ *
+ *  Created on: 02.08.2012
+ *      Author: niko
+ */
+
+#ifndef BETWEENFACTORSWITCHABLE_H_
+#define BETWEENFACTORSWITCHABLE_H_
+
+#include <gtsam/nonlinear/NonlinearFactor.h>
+
+#include <iostream>
+using std::cout;
+using std::endl;
+
+#include "switchVariableLinear.h"
+#include "switchVariableSigmoid.h"
+
+
+namespace vertigo {
+
+  template<class VALUE>
+  class BetweenFactorSwitchableLinear : public gtsam::NoiseModelFactor3<VALUE, VALUE, SwitchVariableLinear>
+  {
+    public:
+      BetweenFactorSwitchableLinear() {};
+      BetweenFactorSwitchableLinear(gtsam::Key key1, gtsam::Key key2, gtsam::Key key3, const VALUE& measured, const gtsam::SharedNoiseModel& model)
+      : gtsam::NoiseModelFactor3<VALUE, VALUE, SwitchVariableLinear>(model, key1, key2, key3),
+        betweenFactor(key1, key2, measured, model) {};
+
+      gtsam::Vector evaluateError(const VALUE& p1, const VALUE& p2, const SwitchVariableLinear& s,
+          boost::optional<gtsam::Matrix&> H1 = boost::none,
+          boost::optional<gtsam::Matrix&> H2 =  boost::none,
+          boost::optional<gtsam::Matrix&> H3 =  boost::none) const
+        {
+
+          // calculate error
+          gtsam::Vector error = betweenFactor.evaluateError(p1, p2, H1, H2);
+          error *= s.value();
+
+          // handle derivatives
+          if (H1) *H1 = *H1 * s.value();
+          if (H2) *H2 = *H2 * s.value();
+          if (H3) *H3 = error;
+
+          return error;
+        };
+
+    private:
+      gtsam::BetweenFactor<VALUE> betweenFactor;
+
+  };
+
+
+
+  template<class VALUE>
+  class BetweenFactorSwitchableSigmoid : public gtsam::NoiseModelFactor3<VALUE, VALUE, SwitchVariableSigmoid>
+  {
+    public:
+      BetweenFactorSwitchableSigmoid() {};
+      BetweenFactorSwitchableSigmoid(gtsam::Key key1, gtsam::Key key2, gtsam::Key key3, const VALUE& measured, const gtsam::SharedNoiseModel& model)
+      : gtsam::NoiseModelFactor3<VALUE, VALUE, SwitchVariableSigmoid>(model, key1, key2, key3),
+        betweenFactor(key1, key2, measured, model) {};
+
+      gtsam::Vector evaluateError(const VALUE& p1, const VALUE& p2, const SwitchVariableSigmoid& s,
+          boost::optional<gtsam::Matrix&> H1 = boost::none,
+          boost::optional<gtsam::Matrix&> H2 =  boost::none,
+          boost::optional<gtsam::Matrix&> H3 =  boost::none) const
+      {
+
+        // calculate error
+        gtsam::Vector error = betweenFactor.evaluateError(p1, p2, H1, H2);
+
+
+        double w = sigmoid(s.value());
+        error *= w;
+
+        // handle derivatives
+        if (H1) *H1 = *H1 * w;
+        if (H2) *H2 = *H2 * w;
+        if (H3) *H3 = error /* (w*(1.0-w))*/;  // sig(x)*(1-sig(x)) is the derivative of sig(x) wrt. x
+
+        return error;
+      };
+
+    private:
+      gtsam::BetweenFactor<VALUE> betweenFactor;
+
+      double sigmoid(double x) const {
+        return 1.0/(1.0+exp(-x));
+      }
+  };
+
+}
+
+#endif /* BETWEENFACTORSWITCHABLE_H_ */

corelib/src/vertigo/gtsam/switchVariableLinear.h

@@ -0,0 +1,130 @@
+/*
+ * switchVariableLinear.h
+ *
+ *  Created on: 02.08.2012
+ *      Author: niko
+ */
+
+#ifndef SWITCHVARIABLELINEAR_H_
+#define SWITCHVARIABLELINEAR_H_
+
+#pragma once
+
+#include <gtsam/base/DerivedValue.h>
+#include <gtsam/base/Lie.h>
+
+namespace vertigo {
+
+  /**
+   * SwitchVariableLinear is a wrapper around double to allow it to be a Lie type
+   */
+  struct SwitchVariableLinear : public gtsam::DerivedValue<SwitchVariableLinear> {
+
+    /** default constructor */
+    SwitchVariableLinear() : d_(0.0) {};
+
+    /** wrap a double */
+    SwitchVariableLinear(double d) : d_(d) {
+     // if (d_ < 0.0) d_=0.0;
+     // else if(d_>1.0) d_=1.0;
+    };
+
+    /** access the underlying value */
+    double value() const { return d_; }
+
+    /** print @param s optional string naming the object */
+    inline void print(const std::string& name="") const {
+      std::cout << name << ": " << d_ << std::endl;
+    }
+
+    /** equality up to tolerance */
+    inline bool equals(const SwitchVariableLinear& expected, double tol=1e-5) const {
+      return fabs(expected.d_ - d_) <= tol;
+    }
+
+    // Manifold requirements
+
+    /** Returns dimensionality of the tangent space */
+    inline size_t dim() const { return 1; }
+    inline static size_t Dim() { return 1; }
+
+    /** Update the SwitchVariableLinear with a tangent space update */
+    inline SwitchVariableLinear retract(const gtsam::Vector& v) const {
+      double x = value() + v(0);
+
+      if (x>1.0) x=1.0;
+      else if (x<0.0) x=0.0;
+
+      return SwitchVariableLinear(x);
+    }
+
+    /** @return the local coordinates of another object */
+    inline gtsam::Vector localCoordinates(const SwitchVariableLinear& t2) const { return gtsam::Vector1(t2.value() - value()); }
+
+    // Group requirements
+
+    /** identity */
+    inline static SwitchVariableLinear identity() {
+      return SwitchVariableLinear();
+    }
+
+    /** compose with another object */
+    inline SwitchVariableLinear compose(const SwitchVariableLinear& p) const {
+      return SwitchVariableLinear(d_ + p.d_);
+    }
+
+    /** between operation */
+    inline SwitchVariableLinear between(const SwitchVariableLinear& l2,
+        boost::optional<gtsam::Matrix&> H1=boost::none,
+        boost::optional<gtsam::Matrix&> H2=boost::none) const {
+      if(H1) *H1 = -gtsam::eye(1);
+      if(H2) *H2 = gtsam::eye(1);
+      return SwitchVariableLinear(l2.value() - value());
+    }
+
+    /** invert the object and yield a new one */
+    inline SwitchVariableLinear inverse() const {
+      return SwitchVariableLinear(-1.0 * value());
+    }
+
+    // Lie functions
+
+    /** Expmap around identity */
+    static inline SwitchVariableLinear Expmap(const gtsam::Vector& v) { return SwitchVariableLinear(v(0)); }
+
+    /** Logmap around identity - just returns with default cast back */
+    static inline gtsam::Vector Logmap(const SwitchVariableLinear& p) { return gtsam::Vector1(p.value()); }
+
+  private:
+      double d_;
+  };
+}
+
+namespace gtsam {
+// Define Key to be Testable by specializing gtsam::traits
+template<typename T> struct traits;
+template<> struct traits<vertigo::SwitchVariableLinear> {
+  static void Print(const vertigo::SwitchVariableLinear& key, const std::string& str = "") {
+    key.print(str);
+  }
+  static bool Equals(const vertigo::SwitchVariableLinear& key1, const vertigo::SwitchVariableLinear& key2, double tol = 1e-8) {
+    return key1.equals(key2, tol);
+  }
+  static int GetDimension(const vertigo::SwitchVariableLinear & key) {return key.Dim();}
+
+  typedef OptionalJacobian<3, 3> ChartJacobian;
+  typedef gtsam::Vector TangentVector;
+  static TangentVector Local(const vertigo::SwitchVariableLinear& origin, const vertigo::SwitchVariableLinear& other,
+  ChartJacobian Horigin = boost::none, ChartJacobian Hother = boost::none) {
+    return origin.localCoordinates(other);
+  }
+  static vertigo::SwitchVariableLinear Retract(const vertigo::SwitchVariableLinear& g, const TangentVector& v,
+        ChartJacobian H1 = boost::none, ChartJacobian H2 = boost::none) {
+      return g.retract(v);
+    }
+};
+}
+
+
+
+#endif /* SWITCHVARIABLELINEAR_H_ */

corelib/src/vertigo/gtsam/switchVariableSigmoid.h

@@ -0,0 +1,129 @@
+/*
+ * switchVariableSigmoid.h
+ *
+ *  Created on: 08.08.2012
+ *      Author: niko
+ */
+
+#ifndef SWITCHVARIABLESIGMOID_H_
+#define SWITCHVARIABLESIGMOID_H_
+
+#pragma once
+
+#include <gtsam/base/DerivedValue.h>
+#include <gtsam/base/Lie.h>
+
+namespace vertigo {
+
+  /**
+   * SwitchVariableSigmoid is a wrapper around double to allow it to be a Lie type
+   */
+  struct SwitchVariableSigmoid : public gtsam::DerivedValue<SwitchVariableSigmoid> {
+
+    /** default constructor */
+    SwitchVariableSigmoid() : d_(10.0) {};
+
+    /** wrap a double */
+    SwitchVariableSigmoid(double d) : d_(d) {
+      if (d_ < -10.0) d_=-10.0;
+      else if(d_>10.0) d_=10.0;
+    };
+
+    /** access the underlying value */
+    double value() const { return d_; }
+
+    /** print @param s optional string naming the object */
+    inline void print(const std::string& name="") const {
+      std::cout << name << ": " << d_ << std::endl;
+    }
+
+    /** equality up to tolerance */
+    inline bool equals(const SwitchVariableSigmoid& expected, double tol=1e-5) const {
+      return fabs(expected.d_ - d_) <= tol;
+    }
+
+    // Manifold requirements
+
+    /** Returns dimensionality of the tangent space */
+    inline size_t dim() const { return 1; }
+    inline static size_t Dim() { return 1; }
+
+    /** Update the SwitchVariableSigmoid with a tangent space update */
+    inline SwitchVariableSigmoid retract(const gtsam::Vector& v) const {
+      double x = value() + v(0);
+
+      if (x>10.0) x=10.0;
+      else if (x<-10.0) x=-10.0;
+
+      return SwitchVariableSigmoid(x);
+    }
+
+    /** @return the local coordinates of another object */
+    inline gtsam::Vector localCoordinates(const SwitchVariableSigmoid& t2) const { return gtsam::Vector1(t2.value() - value()); }
+
+    // Group requirements
+
+    /** identity */
+    inline static SwitchVariableSigmoid identity() {
+      return SwitchVariableSigmoid();
+    }
+
+    /** compose with another object */
+    inline SwitchVariableSigmoid compose(const SwitchVariableSigmoid& p) const {
+      return SwitchVariableSigmoid(d_ + p.d_);
+    }
+
+    /** between operation */
+    inline SwitchVariableSigmoid between(const SwitchVariableSigmoid& l2,
+        boost::optional<gtsam::Matrix&> H1=boost::none,
+        boost::optional<gtsam::Matrix&> H2=boost::none) const {
+      if(H1) *H1 = -gtsam::eye(1);
+      if(H2) *H2 = gtsam::eye(1);
+      return SwitchVariableSigmoid(l2.value() - value());
+    }
+
+    /** invert the object and yield a new one */
+    inline SwitchVariableSigmoid inverse() const {
+      return SwitchVariableSigmoid(-1.0 * value());
+    }
+
+    // Lie functions
+
+    /** Expmap around identity */
+    static inline SwitchVariableSigmoid Expmap(const gtsam::Vector& v) { return SwitchVariableSigmoid(v(0)); }
+
+    /** Logmap around identity - just returns with default cast back */
+    static inline gtsam::Vector Logmap(const SwitchVariableSigmoid& p) { return gtsam::Vector1(p.value()); }
+
+  private:
+      double d_;
+  };
+}
+
+
+namespace gtsam {
+// Define Key to be Testable by specializing gtsam::traits
+template<typename T> struct traits;
+template<> struct traits<vertigo::SwitchVariableSigmoid> {
+  static void Print(const vertigo::SwitchVariableSigmoid& key, const std::string& str = "") {
+    key.print(str);
+  }
+  static bool Equals(const vertigo::SwitchVariableSigmoid& key1, const vertigo::SwitchVariableSigmoid& key2, double tol = 1e-8) {
+    return key1.equals(key2, tol);
+  }
+  static int GetDimension(const vertigo::SwitchVariableSigmoid & key) {return key.Dim();}
+
+  typedef OptionalJacobian<3, 3> ChartJacobian;
+  typedef gtsam::Vector TangentVector;
+  static TangentVector Local(const vertigo::SwitchVariableSigmoid& origin, const vertigo::SwitchVariableSigmoid& other,
+  ChartJacobian Horigin = boost::none, ChartJacobian Hother = boost::none) {
+    return origin.localCoordinates(other);
+  }
+  static vertigo::SwitchVariableSigmoid Retract(const vertigo::SwitchVariableSigmoid& g, const TangentVector& v,
+        ChartJacobian H1 = boost::none, ChartJacobian H2 = boost::none) {
+      return g.retract(v);
+    }
+};
+}
+
+#endif /* SWITCHVARIABLESIGMOID_H_ */

corelib/src/vertigo/readme.txt

@@ -0,0 +1,8 @@
+
+Info: http://openslam.org/vertigo.html
+Source: https://github.com/christiankerl/vertigo/tree/master/trunk
+Commit: fbd438488a56cdba805fa2f75aa3e394e3eda7ff
+License: GPL v3
+
+Tested with g2o (ROS Indigo/2014.02.18)
+Tested with GTSAM commit c73b835

guilib/include/rtabmap/gui/DatabaseViewer.h

@@ -82,6 +82,7 @@ private slots:
 	void extractImages();
 	void generateLocalGraph();
 	void generateTOROGraph();
+	void generateG2OGraph();
 	void view3DMap();
 	void generate3DMap();
 	void detectMoreLoopClosures();

guilib/include/rtabmap/gui/MainWindow.h

@@ -132,6 +132,7 @@ private slots:
 	void exportPosesRGBDSLAM();
 	void exportPosesKITTI();
 	void exportPosesTORO();
+	void exportPosesG2O();
 	void postProcessing();
 	void deleteMemory();
 	void openWorkingDirectory();

guilib/include/rtabmap/gui/PreferencesDialog.h

@@ -301,7 +301,7 @@ private:
 	void addParameter(const QObject * object, double value);
 	void addParameter(const QObject * object, const QString & value);
 	void addParameters(const QObjectList & children);
-	void addParameters(const QStackedWidget * stackedWidget);
+	void addParameters(const QStackedWidget * stackedWidget, int panel = -1);
 	void addParameters(const QGroupBox * box);
 	QList<QGroupBox*> getGroupBoxes();
 	void readSettingsBegin();

guilib/src/AboutDialog.cpp

@@ -61,6 +61,7 @@ AboutDialog::AboutDialog(QWidget * parent) :
 	_ui->label_flycapture2->setText(CameraStereoFlyCapture2::available()?"Yes":"No");
 
 	_ui->label_g2o->setText(graph::G2OOptimizer::available()?"Yes":"No");
+	_ui->label_gtsam->setText(graph::GTSAMOptimizer::available()?"Yes":"No");
 	_ui->label_cvsba->setText(graph::CVSBAOptimizer::available()?"Yes":"No");
 
 }

guilib/src/DatabaseViewer.cpp

@@ -127,6 +127,19 @@ DatabaseViewer::DatabaseViewer(QWidget * parent) :
 			ui_->comboBox_graphOptimizer->setCurrentIndex(0);
 		}
 	}
+	if(!graph::GTSAMOptimizer::available())
+	{
+		ui_->comboBox_graphOptimizer->setItemData(1, 0, Qt::UserRole - 1);
+		if(ui_->comboBox_graphOptimizer->currentIndex() == 2)
+		{
+			UWARN("GTSAM is not available, setting optimization default to TORO.");
+			ui_->comboBox_graphOptimizer->setCurrentIndex(0);
+		}
+	}
+	if(!graph::G2OOptimizer::available() && !graph::GTSAMOptimizer::available())
+	{
+		ui_->checkBox_robust->setEnabled(false);
+	}
 
 	ui_->menuView->addAction(ui_->dockWidget_constraints->toggleViewAction());
 	ui_->menuView->addAction(ui_->dockWidget_graphView->toggleViewAction());
@@ -146,6 +159,8 @@ DatabaseViewer::DatabaseViewer(QWidget * parent) :
 	connect(ui_->actionGenerate_graph_dot, SIGNAL(triggered()), this, SLOT(generateGraph()));
 	connect(ui_->actionGenerate_local_graph_dot, SIGNAL(triggered()), this, SLOT(generateLocalGraph()));
 	connect(ui_->actionGenerate_TORO_graph_graph, SIGNAL(triggered()), this, SLOT(generateTOROGraph()));
+	connect(ui_->actionGenerate_g2o_graph_g2o, SIGNAL(triggered()), this, SLOT(generateG2OGraph()));
+	ui_->actionGenerate_g2o_graph_g2o->setEnabled(graph::G2OOptimizer::available());
 	connect(ui_->actionView_3D_map, SIGNAL(triggered()), this, SLOT(view3DMap()));
 	connect(ui_->actionGenerate_3D_map_pcd, SIGNAL(triggered()), this, SLOT(generate3DMap()));
 	connect(ui_->actionDetect_more_loop_closures, SIGNAL(triggered()), this, SLOT(detectMoreLoopClosures()));
@@ -168,6 +183,7 @@ DatabaseViewer::DatabaseViewer(QWidget * parent) :
 	ui_->pushButton_reject->setEnabled(false);
 
 	ui_->actionGenerate_TORO_graph_graph->setEnabled(false);
+	ui_->actionGenerate_g2o_graph_g2o->setEnabled(false);
 
 	ui_->horizontalSlider_A->setTracking(false);
 	ui_->horizontalSlider_B->setTracking(false);
@@ -198,6 +214,7 @@ DatabaseViewer::DatabaseViewer(QWidget * parent) :
 	connect(ui_->spinBox_iterations, SIGNAL(editingFinished()), this, SLOT(updateGraphView()));
 	connect(ui_->spinBox_optimizationsFrom, SIGNAL(editingFinished()), this, SLOT(updateGraphView()));
 	connect(ui_->checkBox_spanAllMaps, SIGNAL(stateChanged(int)), this, SLOT(updateGraphView()));
+	connect(ui_->checkBox_robust, SIGNAL(stateChanged(int)), this, SLOT(updateGraphView()));
 	connect(ui_->checkBox_ignoreCovariance, SIGNAL(stateChanged(int)), this, SLOT(updateGraphView()));
 	connect(ui_->checkBox_ignorePoseCorrection, SIGNAL(stateChanged(int)), this, SLOT(updateGraphView()));
 	connect(ui_->checkBox_ignoreGlobalLoop, SIGNAL(stateChanged(int)), this, SLOT(updateGraphView()));
@@ -239,6 +256,7 @@ DatabaseViewer::DatabaseViewer(QWidget * parent) :
 	// Graph view
 	connect(ui_->spinBox_iterations, SIGNAL(valueChanged(int)), this, SLOT(configModified()));
 	connect(ui_->checkBox_spanAllMaps, SIGNAL(stateChanged(int)), this, SLOT(configModified()));
+	connect(ui_->checkBox_robust, SIGNAL(stateChanged(int)), this, SLOT(configModified()));
 	connect(ui_->checkBox_ignoreCovariance, SIGNAL(stateChanged(int)), this, SLOT(configModified()));
 	connect(ui_->checkBox_ignorePoseCorrection, SIGNAL(stateChanged(int)), this, SLOT(configModified()));
 	connect(ui_->checkBox_ignoreGlobalLoop, SIGNAL(stateChanged(int)), this, SLOT(configModified()));
@@ -358,6 +376,7 @@ void DatabaseViewer::readSettings()
 	settings.beginGroup("optimization");
 	ui_->spinBox_iterations->setValue(settings.value("iterations", ui_->spinBox_iterations->value()).toInt());
 	ui_->checkBox_spanAllMaps->setChecked(settings.value("spanToAllMaps", ui_->checkBox_spanAllMaps->isChecked()).toBool());
+	ui_->checkBox_robust->setChecked(settings.value("robust", ui_->checkBox_robust->isChecked()).toBool());
 	ui_->checkBox_ignoreCovariance->setChecked(settings.value("ignoreCovariance", ui_->checkBox_ignoreCovariance->isChecked()).toBool());
 	ui_->checkBox_ignorePoseCorrection->setChecked(settings.value("ignorePoseCorrection", ui_->checkBox_ignorePoseCorrection->isChecked()).toBool());
 	ui_->checkBox_ignoreGlobalLoop->setChecked(settings.value("ignoreGlobalLoop", ui_->checkBox_ignoreGlobalLoop->isChecked()).toBool());
@@ -451,6 +470,7 @@ void DatabaseViewer::writeSettings()
 	settings.beginGroup("optimization");
 	settings.setValue("iterations", ui_->spinBox_iterations->value());
 	settings.setValue("spanToAllMaps", ui_->checkBox_spanAllMaps->isChecked());
+	settings.setValue("robust", ui_->checkBox_robust->isChecked());
 	settings.setValue("ignoreCovariance", ui_->checkBox_ignoreCovariance->isChecked());
 	settings.setValue("ignorePoseCorrection", ui_->checkBox_ignorePoseCorrection->isChecked());
 	settings.setValue("ignoreGlobalLoop", ui_->checkBox_ignoreGlobalLoop->isChecked());
@@ -556,7 +576,9 @@ bool DatabaseViewer::openDatabase(const QString & path)
 			linksRefined_.clear();
 			linksRemoved_.clear();
 			localMaps_.clear();
+			ui_->graphViewer->clearAll();
 			ui_->actionGenerate_TORO_graph_graph->setEnabled(false);
+			ui_->actionGenerate_g2o_graph_g2o->setEnabled(false);
 			ui_->checkBox_showOptimized->setEnabled(false);
 			databaseFileName_.clear();
 		}
@@ -1057,6 +1079,7 @@ void DatabaseViewer::updateIds()
 	}
 
 	ui_->actionGenerate_TORO_graph_graph->setEnabled(false);
+	ui_->actionGenerate_g2o_graph_g2o->setEnabled(false);
 	graphes_.clear();
 	graphLinks_.clear();
 	neighborLinks_.clear();
@@ -1193,7 +1216,51 @@ void DatabaseViewer::generateTOROGraph()
 		QString path = QFileDialog::getSaveFileName(this, tr("Save File"), pathDatabase_+"/constraints" + QString::number(id) + ".graph", tr("TORO file (*.graph)"));
 		if(!path.isEmpty())
 		{
-			graph::TOROOptimizer::saveGraph(path.toStdString(), uValueAt(graphes_, id), graphLinks_);
+			if(ui_->checkBox_ignoreCovariance->isChecked())
+			{
+				std::multimap<int, rtabmap::Link> links = graphLinks_;
+				for(std::multimap<int, rtabmap::Link>::iterator iter=links.begin(); iter!=links.end(); ++iter)
+				{
+					iter->second.setInfMatrix(cv::Mat::eye(6,6,CV_64FC1));
+				}
+				graph::TOROOptimizer::saveGraph(path.toStdString(), uValueAt(graphes_, id), links);
+			}
+			else
+			{
+				graph::TOROOptimizer::saveGraph(path.toStdString(), uValueAt(graphes_, id), graphLinks_);
+			}
+		}
+	}
+}
+
+void DatabaseViewer::generateG2OGraph()
+{
+	if(!graphes_.size() || !graphLinks_.size())
+	{
+		QMessageBox::warning(this, tr("Cannot generate a g2o graph"), tr("No poses or no links..."));
+		return;
+	}
+	bool ok = false;
+	int id = QInputDialog::getInt(this, tr("Which iteration?"), tr("Iteration (0 -> %1)").arg((int)graphes_.size()-1), (int)graphes_.size()-1, 0, (int)graphes_.size()-1, 1, &ok);
+
+	if(ok)
+	{
+		QString path = QFileDialog::getSaveFileName(this, tr("Save File"), pathDatabase_+"/constraints" + QString::number(id) + ".g2o", tr("g2o file (*.g2o)"));
+		if(!path.isEmpty())
+		{
+			if(ui_->checkBox_ignoreCovariance->isChecked())
+			{
+				std::multimap<int, rtabmap::Link> links = graphLinks_;
+				for(std::multimap<int, rtabmap::Link>::iterator iter=links.begin(); iter!=links.end(); ++iter)
+				{
+					iter->second.setInfMatrix(cv::Mat::eye(6,6,CV_64FC1));
+				}
+				graph::G2OOptimizer::saveGraph(path.toStdString(), uValueAt(graphes_, id), links, ui_->checkBox_robust->isChecked());
+			}
+			else
+			{
+				graph::G2OOptimizer::saveGraph(path.toStdString(), uValueAt(graphes_, id), graphLinks_, ui_->checkBox_robust->isChecked());
+			}
 		}
 	}
 }
@@ -2604,6 +2671,7 @@ void DatabaseViewer::updateGraphView()
 		graphes_.push_back(poses);
 
 		ui_->actionGenerate_TORO_graph_graph->setEnabled(true);
+		ui_->actionGenerate_g2o_graph_g2o->setEnabled(true);
 		std::multimap<int, rtabmap::Link> links = links_;
 
 		// filter current map if not spanning to all maps
@@ -2697,13 +2765,32 @@ void DatabaseViewer::updateGraphView()
 		ui_->label_loopClosures->setText(tr("(%1, %2, %3, %4)").arg(totalGlobal).arg(totalLocalSpace).arg(totalLocalTime).arg(totalUser));
 
 		graph::Optimizer * optimizer = 0;
-		if(ui_->comboBox_graphOptimizer->currentIndex() == graph::Optimizer::kTypeG2O)
+		if(ui_->comboBox_graphOptimizer->currentIndex() == graph::Optimizer::kTypeGTSAM)
 		{
-			optimizer = new graph::G2OOptimizer(ui_->spinBox_iterations->value(), ui_->checkBox_2dslam->isChecked(), ui_->checkBox_ignoreCovariance->isChecked());
+			optimizer = new graph::GTSAMOptimizer(
+					ui_->spinBox_iterations->value(),
+					ui_->checkBox_2dslam->isChecked(),
+					ui_->checkBox_ignoreCovariance->isChecked(),
+					0.0,
+					ui_->checkBox_robust->isChecked());
+		}
+		else if(ui_->comboBox_graphOptimizer->currentIndex() == graph::Optimizer::kTypeG2O)
+		{
+			UINFO("ui_->checkBox_robust->isChecked()=%d", ui_->checkBox_robust->isChecked()?1:0);
+			optimizer = new graph::G2OOptimizer(
+					ui_->spinBox_iterations->value(),
+					ui_->checkBox_2dslam->isChecked(),
+					ui_->checkBox_ignoreCovariance->isChecked(),
+					0.0,
+					ui_->checkBox_robust->isChecked());
 		}
 		else
 		{
-			optimizer = new graph::TOROOptimizer(ui_->spinBox_iterations->value(), ui_->checkBox_2dslam->isChecked(), ui_->checkBox_ignoreCovariance->isChecked());
+			optimizer = new graph::TOROOptimizer(
+					ui_->spinBox_iterations->value(),
+					ui_->checkBox_2dslam->isChecked(),
+					ui_->checkBox_ignoreCovariance->isChecked(),
+					0.0);
 		}
 		std::map<int, rtabmap::Transform> posesOut;
 		std::multimap<int, rtabmap::Link> linksOut;
@@ -2723,6 +2810,10 @@ void DatabaseViewer::updateGraphView()
 		graphLinks_ = linksOut;
 		ui_->label_nodes->setNum((int)finalPoses.size());
 		delete optimizer;
+		if(posesOut.size() && finalPoses.empty())
+		{
+			QMessageBox::warning(this, tr("Graph optimization error!"), tr("Graph optimization has failed. See the terminal for potential errors."));
+		}
 	}
 	if(graphes_.size())
 	{
@@ -3006,7 +3097,7 @@ void DatabaseViewer::refineConstraint(int from, int to, bool silent, bool update
 			}
 			if(!updated)
 			{
-				linksRefined_.insert(std::make_pair<int, Link>(newLink.from(), newLink));
+				linksRefined_.insert(std::make_pair(newLink.from(), newLink));
 
 				if(updateGraph)
 				{
@@ -3129,7 +3220,7 @@ void DatabaseViewer::refineConstraintVisually(int from, int to, bool silent, boo
 		}
 		if(!updated)
 		{
-			linksRefined_.insert(std::make_pair<int, Link>(newLink.from(), newLink));
+			linksRefined_.insert(std::make_pair(newLink.from(), newLink));
 
 			if(updateGraph)
 			{

guilib/src/MainWindow.cpp

@@ -308,6 +308,8 @@ MainWindow::MainWindow(PreferencesDialog * prefDialog, QWidget * parent) :
 	connect(_ui->actionRGBD_SLAM_format_txt, SIGNAL(triggered()), this , SLOT(exportPosesRGBDSLAM()));
 	connect(_ui->actionKITTI_format_txt, SIGNAL(triggered()), this , SLOT(exportPosesKITTI()));
 	connect(_ui->actionTORO_graph, SIGNAL(triggered()), this , SLOT(exportPosesTORO()));
+	connect(_ui->actionG2o_g2o, SIGNAL(triggered()), this , SLOT(exportPosesG2O()));
+	_ui->actionG2o_g2o->setVisible(graph::G2OOptimizer::available());
 	connect(_ui->actionDelete_memory, SIGNAL(triggered()), this , SLOT(deleteMemory()));
 	connect(_ui->actionDownload_all_clouds, SIGNAL(triggered()), this , SLOT(downloadAllClouds()));
 	connect(_ui->actionDownload_graph, SIGNAL(triggered()), this , SLOT(downloadPoseGraph()));
@@ -3178,6 +3180,10 @@ void MainWindow::exportPosesTORO()
 {
 	exportPoses(3);
 }
+void MainWindow::exportPosesG2O()
+{
+	exportPoses(4);
+}
 
 void MainWindow::exportPoses(int format)
 {
@@ -3215,14 +3221,14 @@ void MainWindow::exportPoses(int format)
 
 		if(_exportPosesFileName[format].isEmpty())
 		{
-			_exportPosesFileName[format] = _preferencesDialog->getWorkingDirectory() + QDir::separator() + (format==3?"toro.graph":"poses.txt");
+			_exportPosesFileName[format] = _preferencesDialog->getWorkingDirectory() + QDir::separator() + (format==3?"toro.graph":format==4?"poses.g2o":"poses.txt");
 		}
 
 		QString path = QFileDialog::getSaveFileName(
 				this,
 				tr("Save File"),
 				_exportPosesFileName[format],
-				format == 3?tr("TORO file (*.graph)"):tr("Text file (*.txt)"));
+				format == 3?tr("TORO file (*.graph)"):format==4?tr("g2o file (*.g2o)"):tr("Text file (*.txt)"));
 
 		if(!path.isEmpty())
 		{
@@ -3233,7 +3239,7 @@ void MainWindow::exportPoses(int format)
 			_ui->dockWidget_console->show();
 			_ui->widget_console->appendMsg(
 					QString("%1 saved (global=%2, optimized=%3)... %4")
-					.arg(format == 3?"TORO graph":"Poses")
+					.arg(format == 3?"TORO graph":format == 4?"g2o graph":"Poses")
 					.arg(global?"true":"false")
 					.arg(optimized?"true":"false")
 					.arg(_exportPosesFileName[format]));

guilib/src/PreferencesDialog.cpp

@@ -176,6 +176,14 @@ PreferencesDialog::PreferencesDialog(QWidget * parent) :
 	{
 		_ui->graphOptimization_type->setItemData(1, 0, Qt::UserRole - 1);
 	}
+	if(!graph::GTSAMOptimizer::available())
+	{
+		_ui->graphOptimization_type->setItemData(2, 0, Qt::UserRole - 1);
+	}
+	if(!graph::G2OOptimizer::available() && !graph::GTSAMOptimizer::available())
+	{
+		_ui->graphOptimization_robust->setEnabled(false);
+	}
 	if(!CameraOpenni::available())
 	{
 		_ui->comboBox_cameraRGBD->setItemData(0, 0, Qt::UserRole - 1);
@@ -571,6 +579,7 @@ PreferencesDialog::PreferencesDialog(QWidget * parent) :
 	_ui->graphOptimization_covarianceIgnored->setObjectName(Parameters::kRGBDOptimizeVarianceIgnored().c_str());
 	_ui->graphOptimization_fromGraphEnd->setObjectName(Parameters::kRGBDOptimizeFromGraphEnd().c_str());
 	_ui->graphOptimization_stopEpsilon->setObjectName(Parameters::kRGBDOptimizeEpsilon().c_str());
+	_ui->graphOptimization_robust->setObjectName(Parameters::kRGBDOptimizeRobust().c_str());
 
 	_ui->graphPlan_goalReachedRadius->setObjectName(Parameters::kRGBDGoalReachedRadius().c_str());
 	_ui->graphPlan_planWithNearNodesLinked->setObjectName(Parameters::kRGBDPlanVirtualLinks().c_str());
@@ -1854,7 +1863,7 @@ bool PreferencesDialog::validateForm()
 	// optimization strategy
 	if(!graph::G2OOptimizer::available())
 	{
-		if(_ui->graphOptimization_type->currentIndex() > 0)
+		if(_ui->graphOptimization_type->currentIndex() == 1)
 		{
 			QMessageBox::warning(this, tr("Parameter warning"),
 					tr("Selected graph optimization strategy (g2o) is not available. RTAB-Map is not built "
@@ -1862,6 +1871,16 @@ bool PreferencesDialog::validateForm()
 			_ui->graphOptimization_type->setCurrentIndex(graph::Optimizer::kTypeTORO);
 		}
 	}
+	if(!graph::GTSAMOptimizer::available())
+	{
+		if(_ui->graphOptimization_type->currentIndex() == 2)
+		{
+			QMessageBox::warning(this, tr("Parameter warning"),
+					tr("Selected graph optimization strategy (GTSAM) is not available. RTAB-Map is not built "
+					   "with GTSAM. TORO is set instead for graph optimization strategy."));
+			_ui->graphOptimization_type->setCurrentIndex(graph::Optimizer::kTypeTORO);
+		}
+	}
 
 	//verify binary features and nearest neighbor
 	// BOW dictionary type
@@ -2487,6 +2506,17 @@ void PreferencesDialog::setParameter(const std::string & key, const std::string
 						ok = false;
 					}
 				}
+				if(!graph::GTSAMOptimizer::available())
+				{
+					if(valueInt==2 && combo->objectName().toStdString().compare(Parameters::kRGBDOptimizeStrategy()) == 0)
+					{
+						UWARN("Trying to set \"%s\" to GTSAM but RTAB-Map isn't built "
+							  "with GTSAM. Keeping default combo value: %s.",
+							  combo->objectName().toStdString().c_str(),
+							  combo->currentText().toStdString().c_str());
+						ok = false;
+					}
+				}
 				if(ok)
 				{
 					combo->setCurrentIndex(valueInt);
@@ -2650,6 +2680,22 @@ void PreferencesDialog::addParameter(const QObject * object, int value)
 						this->addParameters(_ui->groupBox_loopClosure_icp2);
 					}
 				}
+				else if(comboBox == _ui->loopClosure_estimationType)
+				{
+					this->addParameters(_ui->stackedWidget_loopClosureEstimation, _ui->loopClosure_estimationType->currentIndex());
+				}
+				else if(comboBox == _ui->odom_estimationType)
+				{
+					this->addParameters(_ui->stackedWidget_odomEstimation, _ui->stackedWidget_odomEstimation->currentIndex());
+				}
+				else if(comboBox == _ui->graphOptimization_type)
+				{
+					this->addParameter(_ui->graphOptimization_iterations,        _ui->graphOptimization_iterations->value());
+					this->addParameter(_ui->graphOptimization_covarianceIgnored, _ui->graphOptimization_covarianceIgnored->isChecked());
+					this->addParameter(_ui->graphOptimization_slam2d,            _ui->graphOptimization_slam2d->isChecked());
+					this->addParameter(_ui->graphOptimization_stopEpsilon,       _ui->graphOptimization_stopEpsilon->value());
+					this->addParameter(_ui->graphOptimization_robust,            _ui->graphOptimization_robust->isChecked());
+				}
 			}
 			// Add parameter
 			_parameters.insert(rtabmap::ParametersPair(object->objectName().toStdString(), QString::number(value).toStdString()));
@@ -2816,13 +2862,22 @@ void PreferencesDialog::addParameters(const QObjectList & children)
 	}
 }
 
-void PreferencesDialog::addParameters(const QStackedWidget * stackedWidget)
+void PreferencesDialog::addParameters(const QStackedWidget * stackedWidget, int panel)
 {
 	if(stackedWidget)
 	{
-		for(int i=0; i<stackedWidget->count(); ++i)
+		if(panel == -1)
 		{
-			const QObjectList & children = stackedWidget->widget(i)->children();
+			for(int i=0; i<stackedWidget->count(); ++i)
+			{
+				const QObjectList & children = stackedWidget->widget(i)->children();
+				addParameters(children);
+			}
+		}
+		else
+		{
+			UASSERT(panel<stackedWidget->count());
+			const QObjectList & children = stackedWidget->widget(panel)->children();
 			addParameters(children);
 		}
 	}

guilib/src/ui/DatabaseViewer.ui

@@ -50,8 +50,8 @@
             <rect>
              <x>0</x>
              <y>0</y>
-             <width>154</width>
-             <height>184</height>
+             <width>175</width>
+             <height>173</height>
             </rect>
            </property>
            <layout class="QGridLayout" name="gridLayout" columnstretch="0,1">
@@ -236,8 +236,8 @@
             <rect>
              <x>0</x>
              <y>0</y>
-             <width>154</width>
-             <height>184</height>
+             <width>174</width>
+             <height>173</height>
             </rect>
            </property>
            <layout class="QGridLayout" name="gridLayout_2" columnstretch="0,1">
@@ -418,7 +418,7 @@
      <x>0</x>
      <y>0</y>
      <width>1285</width>
-     <height>22</height>
+     <height>25</height>
     </rect>
    </property>
    <widget class="QMenu" name="menuFile">
@@ -441,6 +441,7 @@
     <addaction name="actionGenerate_graph_dot"/>
     <addaction name="actionGenerate_local_graph_dot"/>
     <addaction name="actionGenerate_TORO_graph_graph"/>
+    <addaction name="actionGenerate_g2o_graph_g2o"/>
     <addaction name="separator"/>
     <addaction name="actionDetect_more_loop_closures"/>
     <addaction name="actionVisual_Refine_all_neighbor_links"/>
@@ -828,15 +829,15 @@
      <item>
       <widget class="QToolBox" name="toolBox">
        <property name="currentIndex">
-        <number>1</number>
+        <number>2</number>
        </property>
        <widget class="QWidget" name="page">
         <property name="geometry">
          <rect>
           <x>0</x>
           <y>0</y>
-          <width>312</width>
-          <height>314</height>
+          <width>314</width>
+          <height>303</height>
          </rect>
         </property>
         <attribute name="label">
@@ -1055,8 +1056,8 @@
          <rect>
           <x>0</x>
           <y>0</y>
-          <width>366</width>
-          <height>361</height>
+          <width>351</width>
+          <height>347</height>
          </rect>
         </property>
         <attribute name="label">
@@ -1344,9 +1345,9 @@
         <property name="geometry">
          <rect>
           <x>0</x>
-          <y>0</y>
-          <width>330</width>
-          <height>304</height>
+          <y>-26</y>
+          <width>333</width>
+          <height>333</height>
          </rect>
         </property>
         <attribute name="label">
@@ -1363,14 +1364,14 @@
            </property>
           </widget>
          </item>
-         <item row="6" column="2">
+         <item row="7" column="2">
           <widget class="QLabel" name="label_48">
            <property name="text">
             <string>Ignore global loop closures</string>
            </property>
           </widget>
          </item>
-         <item row="9" column="0">
+         <item row="10" column="0">
           <widget class="QCheckBox" name="checkBox_ignoreUserLoop">
            <property name="text">
             <string/>
@@ -1419,14 +1420,14 @@
            </property>
           </widget>
          </item>
-         <item row="5" column="2">
+         <item row="6" column="2">
           <widget class="QLabel" name="label_35">
            <property name="text">
             <string>Ignore pose correction</string>
            </property>
           </widget>
          </item>
-         <item row="6" column="0">
+         <item row="7" column="0">
           <widget class="QCheckBox" name="checkBox_ignoreGlobalLoop">
            <property name="text">
             <string/>
@@ -1436,7 +1437,7 @@
            </property>
           </widget>
          </item>
-         <item row="5" column="0">
+         <item row="6" column="0">
           <widget class="QCheckBox" name="checkBox_ignorePoseCorrection">
            <property name="text">
             <string/>
@@ -1446,7 +1447,7 @@
            </property>
           </widget>
          </item>
-         <item row="8" column="0">
+         <item row="9" column="0">
           <widget class="QCheckBox" name="checkBox_ignoreLocalLoopTime">
            <property name="text">
             <string/>
@@ -1456,6 +1457,19 @@
            </property>
           </widget>
          </item>
+         <item row="11" column="0">
+          <spacer name="verticalSpacer_3">
+           <property name="orientation">
+            <enum>Qt::Vertical</enum>
+           </property>
+           <property name="sizeHint" stdset="0">
+            <size>
+             <width>0</width>
+             <height>40</height>
+            </size>
+           </property>
+          </spacer>
+         </item>
          <item row="0" column="0" colspan="2">
           <widget class="QComboBox" name="comboBox_graphOptimizer">
            <property name="sizeAdjustPolicy">
@@ -1471,6 +1485,11 @@
              <string>g2o</string>
             </property>
            </item>
+           <item>
+            <property name="text">
+             <string>GTSAM</string>
+            </property>
+           </item>
           </widget>
          </item>
          <item row="2" column="2">
@@ -1480,34 +1499,21 @@
            </property>
           </widget>
          </item>
-         <item row="9" column="2">
+         <item row="10" column="2">
           <widget class="QLabel" name="label_50">
            <property name="text">
             <string>Ignore user loop closures</string>
            </property>
           </widget>
          </item>
-         <item row="7" column="2">
+         <item row="8" column="2">
           <widget class="QLabel" name="label_47">
            <property name="text">
             <string>Ignore local loop closures (space)</string>
            </property>
           </widget>
          </item>
-         <item row="10" column="0">
-          <spacer name="verticalSpacer_3">
-           <property name="orientation">
-            <enum>Qt::Vertical</enum>
-           </property>
-           <property name="sizeHint" stdset="0">
-            <size>
-             <width>0</width>
-             <height>40</height>
-            </size>
-           </property>
-          </spacer>
-         </item>
-         <item row="7" column="0">
+         <item row="8" column="0">
           <widget class="QCheckBox" name="checkBox_ignoreLocalLoopSpace">
            <property name="text">
             <string/>
@@ -1517,7 +1523,7 @@
            </property>
           </widget>
          </item>
-         <item row="8" column="2">
+         <item row="9" column="2">
           <widget class="QLabel" name="label_49">
            <property name="text">
             <string>Ignore local loop closures (time)</string>
@@ -1531,7 +1537,7 @@
            </property>
           </widget>
          </item>
-         <item row="4" column="0">
+         <item row="5" column="0">
           <widget class="QCheckBox" name="checkBox_ignoreCovariance">
            <property name="text">
             <string/>
@@ -1548,13 +1554,30 @@
            </property>
           </widget>
          </item>
-         <item row="4" column="2">
+         <item row="5" column="2">
           <widget class="QLabel" name="label_34">
            <property name="text">
             <string>Ignore covariance</string>
            </property>
           </widget>
          </item>
+         <item row="4" column="2">
+          <widget class="QLabel" name="label_63">
+           <property name="text">
+            <string>Robust optimization</string>
+           </property>
+          </widget>
+         </item>
+         <item row="4" column="0">
+          <widget class="QCheckBox" name="checkBox_robust">
+           <property name="text">
+            <string/>
+           </property>
+           <property name="checked">
+            <bool>false</bool>
+           </property>
+          </widget>
+         </item>
         </layout>
        </widget>
        <widget class="QWidget" name="page_4">
@@ -1562,8 +1585,8 @@
          <rect>
           <x>0</x>
           <y>0</y>
-          <width>248</width>
-          <height>319</height>
+          <width>243</width>
+          <height>284</height>
          </rect>
         </property>
         <attribute name="label">
@@ -1757,7 +1780,7 @@
           <x>0</x>
           <y>0</y>
           <width>201</width>
-          <height>126</height>
+          <height>117</height>
          </rect>
         </property>
         <attribute name="label">
@@ -1856,8 +1879,8 @@
          <rect>
           <x>0</x>
           <y>0</y>
-          <width>283</width>
-          <height>322</height>
+          <width>285</width>
+          <height>309</height>
          </rect>
         </property>
         <attribute name="label">
@@ -2301,6 +2324,11 @@
     <string>Reset all changes</string>
    </property>
   </action>
+  <action name="actionGenerate_g2o_graph_g2o">
+   <property name="text">
+    <string>Generate g2o graph (*.g2o)...</string>
+   </property>
+  </action>
  </widget>
  <customwidgets>
   <customwidget>

guilib/src/ui/aboutDialog.ui

@@ -82,6 +82,13 @@ p, li { white-space: pre-wrap; }
         </item>
         <item>
          <layout class="QGridLayout" name="gridLayout" columnstretch="0,1">
+          <item row="10" column="0">
+           <widget class="QLabel" name="label_12">
+            <property name="text">
+             <string>With Freenect :</string>
+            </property>
+           </widget>
+          </item>
           <item row="11" column="0">
            <widget class="QLabel" name="label_13">
             <property name="text">
@@ -205,13 +212,6 @@ p, li { white-space: pre-wrap; }
             </property>
            </widget>
           </item>
-          <item row="10" column="0">
-           <widget class="QLabel" name="label_12">
-            <property name="text">
-             <string>With Freenect :</string>
-            </property>
-           </widget>
-          </item>
           <item row="15" column="0">
            <widget class="QLabel" name="label_14">
             <property name="text">
@@ -293,14 +293,14 @@ p, li { white-space: pre-wrap; }
             </property>
            </widget>
           </item>
-          <item row="16" column="0">
+          <item row="17" column="0">
            <widget class="QLabel" name="label_18">
             <property name="text">
              <string>With cvsba :</string>
             </property>
            </widget>
           </item>
-          <item row="16" column="1">
+          <item row="17" column="1">
            <widget class="QLabel" name="label_cvsba">
             <property name="text">
              <string/>
@@ -310,6 +310,23 @@ p, li { white-space: pre-wrap; }
             </property>
            </widget>
           </item>
+          <item row="16" column="0">
+           <widget class="QLabel" name="label_19">
+            <property name="text">
+             <string>With GTSAM :</string>
+            </property>
+           </widget>
+          </item>
+          <item row="16" column="1">
+           <widget class="QLabel" name="label_gtsam">
+            <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">
@@ -64,6 +64,7 @@
       <addaction name="actionRGBD_SLAM_format_txt"/>
       <addaction name="actionKITTI_format_txt"/>
       <addaction name="actionTORO_graph"/>
+      <addaction name="actionG2o_g2o"/>
      </widget>
      <addaction name="actionOpen_working_directory"/>
      <addaction name="actionDump_the_memory"/>
@@ -1259,6 +1260,11 @@
     <string>Raw format (*.txt)</string>
    </property>
   </action>
+  <action name="actionG2o_g2o">
+   <property name="text">
+    <string>g2o (*.g2o)</string>
+   </property>
+  </action>
  </widget>
  <customwidgets>
   <customwidget>

guilib/src/ui/preferencesDialog.ui

@@ -64,8 +64,8 @@
            <rect>
             <x>0</x>
             <y>-392</y>
-            <width>755</width>
-            <height>1591</height>
+            <width>760</width>
+            <height>1570</height>
            </rect>
           </property>
           <layout class="QVBoxLayout" name="verticalLayout_16">
@@ -86,7 +86,7 @@
                 <enum>QFrame::Raised</enum>
                </property>
                <property name="currentIndex">
-                <number>3</number>
+                <number>19</number>
                </property>
                <widget class="QWidget" name="page_22">
                 <layout class="QVBoxLayout" name="verticalLayout_29">
@@ -6553,6 +6553,11 @@ If set to false, classic RTAB-Map loop closure detection is done using only imag
                            <string>g2o</string>
                           </property>
                          </item>
+                         <item>
+                          <property name="text">
+                           <string>GTSAM</string>
+                          </property>
+                         </item>
                         </widget>
                        </item>
                        <item row="1" column="0">
@@ -6575,14 +6580,14 @@ If set to false, classic RTAB-Map loop closure detection is done using only imag
                          </property>
                         </widget>
                        </item>
-                       <item row="6" column="0">
+                       <item row="7" column="0">
                         <widget class="QCheckBox" name="graphOptimization_fromGraphEnd">
                          <property name="text">
                           <string/>
                          </property>
                         </widget>
                        </item>
-                       <item row="6" column="1">
+                       <item row="7" column="1">
                         <widget class="QLabel" name="label_151">
                          <property name="text">
                           <string>Optimize graph from the newest node.</string>
@@ -6595,7 +6600,7 @@ If set to false, classic RTAB-Map loop closure detection is done using only imag
                          </property>
                         </widget>
                        </item>
-                       <item row="5" column="1">
+                       <item row="6" column="1">
                         <widget class="Line" name="line">
                          <property name="orientation">
                           <enum>Qt::Horizontal</enum>
@@ -6632,7 +6637,7 @@ If set to false, classic RTAB-Map loop closure detection is done using only imag
                          </property>
                         </widget>
                        </item>
-                       <item row="8" column="1">
+                       <item row="9" column="1">
                         <widget class="QLabel" name="label_183">
                          <property name="text">
                           <string>-If false, the graph is optimized from the oldest node of the current graph. It can be useful to preserve the map referential from the oldest node. An odometry correction between frames /map to /odom is computed. Warning: when some nodes are transferred, the first referential of the local map may change, resulting in momentary changes in robot/map position (which are annoying in teleoperation).</string>
@@ -6645,7 +6650,7 @@ If set to false, classic RTAB-Map loop closure detection is done using only imag
                          </property>
                         </widget>
                        </item>
-                       <item row="7" column="1">
+                       <item row="8" column="1">
                         <widget class="QLabel" name="label_211">
                          <property name="text">
                           <string>-If true, there is no odometry correction computed. All previous poses in the map are corrected instead, not the last one (which corresponds to latest odometry value). So, the transform between frames /map to /odom will be always Identity even on loop closures.</string>
@@ -6710,6 +6715,26 @@ If set to false, classic RTAB-Map loop closure detection is done using only imag
                          </property>
                         </widget>
                        </item>
+                       <item row="5" column="0">
+                        <widget class="QCheckBox" name="graphOptimization_robust">
+                         <property name="text">
+                          <string/>
+                         </property>
+                        </widget>
+                       </item>
+                       <item row="5" column="1">
+                        <widget class="QLabel" name="label_258">
+                         <property name="text">
+                          <string>Robust graph optimization using Vertigo (only for g2o and GTSAM optimization strategies). This approach can filter wrong loop closure detections.</string>
+                         </property>
+                         <property name="wordWrap">
+                          <bool>true</bool>
+                         </property>
+                         <property name="textInteractionFlags">
+                          <set>Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse</set>
+                         </property>
+                        </widget>
+                       </item>
                       </layout>
                      </widget>
                     </item>