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

2026-09-01 17:10:26 +08:00 · 2015-09-02 17:51:43 -04:00
parent a2b9c9f9a0
commit 7a721105ae
40 changed files with 2125 additions and 158 deletions
--- a/corelib/src/CMakeLists.txt
+++ b/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 
--- a/corelib/src/Graph.cpp
+++ b/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
 //////////////////////
--- a/corelib/src/Rtabmap.cpp
+++ b/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...");
+		}
 	}
 }

--- a/corelib/src/Transform.cpp
+++ b/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 &&
--- a/corelib/src/sqlite3/readme.txt
+++ b/corelib/src/sqlite3/readme.txt
@@ -0,0 +1,3 @@
+
+Info: https://www.sqlite.org/
+License: Public domain (https://www.sqlite.org/copyright.html)
--- a/corelib/src/toro3d/readme.txt
+++ b/corelib/src/toro3d/readme.txt
@@ -0,0 +1,3 @@
+
+Info: https://www.openslam.org/toro.html
+License: Creative Commons (Attribution-NonCommercial-ShareAlike)
--- a/corelib/src/vertigo/g2o/edge_se2MaxMixture.cpp
+++ b/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
+
+
+
--- a/corelib/src/vertigo/g2o/edge_se2MaxMixture.h
+++ b/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_ */
--- a/corelib/src/vertigo/g2o/edge_se2Switchable.cpp
+++ b/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
+
--- a/corelib/src/vertigo/g2o/edge_se2Switchable.h
+++ b/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_ */
--- a/corelib/src/vertigo/g2o/edge_se3Switchable.cpp
+++ b/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
--- a/corelib/src/vertigo/g2o/edge_se3Switchable.h
+++ b/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_ */
--- a/corelib/src/vertigo/g2o/edge_switchPrior.cpp
+++ b/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();
+
+    }
+
--- a/corelib/src/vertigo/g2o/edge_switchPrior.h
+++ b/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();
+};
--- a/corelib/src/vertigo/g2o/types_g2o_robust.cpp
+++ b/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
+
--- a/corelib/src/vertigo/g2o/vertex_switchLinear.cpp
+++ b/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;
+    }
--- a/corelib/src/vertigo/g2o/vertex_switchLinear.h
+++ b/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;
+
+    };
--- a/corelib/src/vertigo/gtsam/betweenFactorMaxMix.h
+++ b/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_ */
--- a/corelib/src/vertigo/gtsam/betweenFactorSwitchable.h
+++ b/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_ */
--- a/corelib/src/vertigo/gtsam/switchVariableLinear.h
+++ b/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_ */
--- a/corelib/src/vertigo/gtsam/switchVariableSigmoid.h
+++ b/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_ */
--- a/corelib/src/vertigo/readme.txt
+++ b/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