diff --git a/corelib/include/rtabmap/core/Optimizer.h b/corelib/include/rtabmap/core/Optimizer.h
index e519e41d..7ded6667 100644
--- a/corelib/include/rtabmap/core/Optimizer.h
+++ b/corelib/include/rtabmap/core/Optimizer.h
@@ -69,13 +69,12 @@ public:
 	static Optimizer * create(Optimizer::Type type, const ParametersMap & parameters = ParametersMap());
 
 	// Get connected poses and constraints from a set of links
-	static void getConnectedGraph(
+	void getConnectedGraph(
 			int fromId,
 			const std::map<int, Transform> & posesIn,
-			const std::multimap<int, Link> & linksIn, // only one link between two poses
+			const std::multimap<int, Link> & linksIn,
 			std::map<int, Transform> & posesOut,
-			std::multimap<int, Link> & linksOut,
-			int depth = 0);
+			std::multimap<int, Link> & linksOut) const;
 
 public:
 	virtual ~Optimizer() {}
diff --git a/corelib/src/Optimizer.cpp b/corelib/src/Optimizer.cpp
index 6f1dc908..d67a46aa 100644
--- a/corelib/src/Optimizer.cpp
+++ b/corelib/src/Optimizer.cpp
@@ -159,94 +159,107 @@ void Optimizer::getConnectedGraph(
 		const std::map<int, Transform> & posesIn,
 		const std::multimap<int, Link> & linksIn,
 		std::map<int, Transform> & posesOut,
-		std::multimap<int, Link> & linksOut,
-		int depth)
+		std::multimap<int, Link> & linksOut) const
 {
-	UASSERT(depth >= 0);
+	UDEBUG("IN: fromId=%d poses=%d links=%d priorsIgnored=%d landmarksIgnored=%d", fromId, (int)posesIn.size(), (int)linksIn.size(), priorsIgnored()?1:0, landmarksIgnored()?1:0);
 	UASSERT(fromId>0);
 	UASSERT(uContains(posesIn, fromId));
 
 	posesOut.clear();
 	linksOut.clear();
 
-	std::set<int> curentPoses;
 	std::set<int> nextPoses;
 	nextPoses.insert(fromId);
-	int d = 0;
 	std::multimap<int, int> biLinks;
 	for(std::multimap<int, Link>::const_iterator iter=linksIn.begin(); iter!=linksIn.end(); ++iter)
 	{
 		if(iter->second.from() != iter->second.to())
 		{
-			UASSERT_MSG(graph::findLink(biLinks, iter->second.from(), iter->second.to()) == biLinks.end(),
-					uFormat("Input links should be unique between two poses (%d->%d).",
-							iter->second.from(), iter->second.to()).c_str());
-			biLinks.insert(std::make_pair(iter->second.from(), iter->second.to()));
-			if(iter->second.from() != iter->second.to())
+			if(graph::findLink(biLinks, iter->second.from(), iter->second.to()) == biLinks.end())
 			{
+				biLinks.insert(std::make_pair(iter->second.from(), iter->second.to()));
 				biLinks.insert(std::make_pair(iter->second.to(), iter->second.from()));
 			}
 		}
 	}
 
-	while((depth == 0 || d < depth) && nextPoses.size())
+	while(nextPoses.size())
 	{
-		curentPoses = nextPoses;
-		nextPoses.clear();
+		int fromId = *nextPoses.rbegin(); // fill up all nodes before landmarks
+		nextPoses.erase(*nextPoses.rbegin());
 
-		for(std::set<int>::iterator jter = curentPoses.begin(); jter!=curentPoses.end(); ++jter)
+		if(posesOut.empty())
 		{
-			int fromId = *jter;
-			if(posesOut.empty())
+			posesOut.insert(std::make_pair(fromId, posesIn.find(fromId)->second));
+
+			// add prior links
+			for(std::multimap<int, Link>::const_iterator pter=linksIn.find(fromId); pter!=linksIn.end() && pter->first==fromId; ++pter)
 			{
-				posesOut.insert(*posesIn.find(fromId));
-				// add prior links
-				for(std::multimap<int, Link>::const_iterator pter=linksIn.find(fromId); pter!=linksIn.end() && pter->first==fromId; ++pter)
+				if(pter->second.from() == pter->second.to() && (!priorsIgnored() || pter->second.type() != Link::kPosePrior))
 				{
-					if(pter->second.from() == pter->second.to())
-					{
-						linksOut.insert(*pter);
-					}
+					linksOut.insert(*pter);
 				}
 			}
+		}
 
-			for(std::multimap<int, int>::const_iterator iter=biLinks.find(fromId); iter!=biLinks.end() && iter->first==fromId; ++iter)
+		for(std::multimap<int, int>::const_iterator iter=biLinks.find(fromId); iter!=biLinks.end() && iter->first==fromId; ++iter)
+		{
+			int toId = iter->second;
+			if(posesIn.find(toId) != posesIn.end() && (!landmarksIgnored() || toId>0))
 			{
-				int toId = iter->second;
-				if(posesIn.find(toId) != posesIn.end())
+				std::multimap<int, Link>::const_iterator kter = graph::findLink(linksIn, fromId, toId);
+				if(nextPoses.find(toId) == nextPoses.end())
 				{
-					std::multimap<int, Link>::const_iterator kter = graph::findLink(linksIn, fromId, toId);
-					int nextDepth = toId!=fromId?depth-1:depth;
-					if(depth == 0 || d < nextDepth || curentPoses.find(toId) != curentPoses.end())
+					if(!uContains(posesOut, toId))
 					{
-						if(!uContains(posesOut, toId))
+						if(isSlam2d() && kter->second.type() == Link::kLandmark && toId>0)
 						{
-							posesOut.insert(std::make_pair(toId, posesOut.at(fromId) * (kter->second.from()==fromId?kter->second.transform():kter->second.transform().inverse())));
-							// add prior links
-							for(std::multimap<int, Link>::const_iterator pter=linksIn.find(toId); pter!=linksIn.end() && pter->first==toId; ++pter)
+							Transform t;
+							if(kter->second.from()==fromId)
 							{
-								if(pter->second.from() == pter->second.to())
-								{
-									linksOut.insert(*pter);
-								}
+								t = kter->second.transform();
 							}
-
-							if(curentPoses.find(toId) == curentPoses.end())
+							else
 							{
-								nextPoses.insert(toId);
+								t = kter->second.transform().inverse();
+							}
+							posesOut.insert(std::make_pair(toId, (posesOut.at(fromId) * t).to3DoF()));
+						}
+						else
+						{
+							Transform t = posesOut.at(fromId) * (kter->second.from()==fromId?kter->second.transform():kter->second.transform().inverse());
+							posesOut.insert(std::make_pair(toId, t));
+						}
+						// add prior links
+						for(std::multimap<int, Link>::const_iterator pter=linksIn.find(toId); pter!=linksIn.end() && pter->first==toId; ++pter)
+						{
+							if(pter->second.from() == pter->second.to() && (!priorsIgnored() || pter->second.type() != Link::kPosePrior))
+							{
+								linksOut.insert(*pter);
 							}
 						}
-						if(graph::findLink(linksOut, fromId, toId) == linksOut.end())
+
+						nextPoses.insert(toId);
+					}
+
+					// only add unique links
+					if(graph::findLink(linksOut, fromId, toId) == linksOut.end())
+					{
+						if(kter->second.to() < 0)
+						{
+							// For landmarks, make sure fromId is the landmark
+							linksOut.insert(std::make_pair(kter->second.to(), kter->second.inverse()));
+						}
+						else
 						{
-							// only add unique links
 							linksOut.insert(*kter);
 						}
 					}
 				}
 			}
 		}
-		++d;
 	}
+	UDEBUG("OUT: poses=%d links=%d", (int)posesOut.size(), (int)linksOut.size());
 }
 
 Optimizer::Optimizer(int iterations, bool slam2d, bool covarianceIgnored, double epsilon, bool robust, bool priorsIgnored, bool landmarksIgnored, float gravitySigma) :
diff --git a/corelib/src/Rtabmap.cpp b/corelib/src/Rtabmap.cpp
index 9073fe4f..fe28a83c 100644
--- a/corelib/src/Rtabmap.cpp
+++ b/corelib/src/Rtabmap.cpp
@@ -3970,10 +3970,10 @@ std::map<int, Transform> Rtabmap::optimizeGraph(
 {
 	UTimer timer;
 	std::map<int, Transform> optimizedPoses;
-	std::map<int, Transform> poses, posesOut;
-	std::multimap<int, Link> edgeConstraints, linksOut;
+	std::map<int, Transform> poses;
+	std::multimap<int, Link> edgeConstraints;
 	UDEBUG("ids=%d", (int)ids.size());
-	_memory->getMetricConstraints(ids, poses, edgeConstraints, lookInDatabase, true);
+	_memory->getMetricConstraints(ids, poses, edgeConstraints, lookInDatabase, !_graphOptimizer->landmarksIgnored());
 	UINFO("get constraints (ids=%d, %d poses, %d edges) time %f s", (int)ids.size(), (int)poses.size(), (int)edgeConstraints.size(), timer.ticks());
 
 	if(_graphOptimizer->iterations() > 0)
@@ -3995,78 +3995,40 @@ std::map<int, Transform> Rtabmap::optimizeGraph(
 		}
 	}
 
-	bool hasLandmarks = poses.begin()->first < 0;
-
-	// The constraints must be all already connected! Only check in debug
-	if(ULogger::level() == ULogger::kDebug)
-	{
-		_graphOptimizer->getConnectedGraph(fromId, poses, edgeConstraints, posesOut, linksOut);
-		if(poses.size() != posesOut.size())
-		{
-			for(std::map<int, Transform>::iterator iter=poses.begin(); iter!=poses.end(); ++iter)
-			{
-				if(posesOut.find(iter->first) == posesOut.end())
-				{
-					UERROR("Not found %d in posesOut", iter->first);
-					for(std::multimap<int, Link>::iterator jter=edgeConstraints.begin(); jter!=edgeConstraints.end(); ++jter)
-					{
-						if(jter->second.from() == iter->first || jter->second.to()==iter->first)
-						{
-							UERROR("Found link %d->%d", jter->second.from(), jter->second.to());
-						}
-					}
-				}
-			}
-		}
-		int ignoredLinks = 0;
-		if(edgeConstraints.size() != linksOut.size())
-		{
-			for(std::multimap<int, Link>::iterator iter=edgeConstraints.begin(); iter!=edgeConstraints.end(); ++iter)
-			{
-				if(graph::findLink(linksOut, iter->second.from(), iter->second.to()) == linksOut.end())
-				{
-					if(iter->second.type() == Link::kPosePrior)
-					{
-						++ignoredLinks;
-					}
-					else
-					{
-						UERROR("Not found link %d->%d in linksOut", iter->second.from(), iter->second.to());
-					}
-				}
-			}
-		}
-		UDEBUG("nodes %d->%d, links %d->%d (ignored=%d)", poses.size(), posesOut.size(), edgeConstraints.size(), linksOut.size(), ignoredLinks);
-		UASSERT_MSG(poses.size() == posesOut.size() && edgeConstraints.size()-ignoredLinks == linksOut.size(),
-				uFormat("nodes %d->%d, links %d->%d (ignored=%d)", poses.size(), posesOut.size(), edgeConstraints.size(), linksOut.size(), ignoredLinks).c_str());
-	}
-
-	if(constraints)
-	{
-		*constraints = edgeConstraints;
-	}
 
 	UASSERT(_graphOptimizer!=0);
 	if(_graphOptimizer->iterations() == 0)
 	{
 		// Optimization disabled! Return not optimized poses.
 		optimizedPoses = poses;
+		if(constraints)
+		{
+			*constraints = edgeConstraints;
+		}
 	}
 	else
 	{
+		bool hasLandmarks = edgeConstraints.begin()->first < 0;
 		if(poses.size() != guessPoses.size() || hasLandmarks)
 		{
-			// recompute poses using only links (robust to multi-session)
+			UDEBUG("recompute poses using only links (robust to multi-session)");
 			std::map<int, Transform> posesOut;
 			std::multimap<int, Link> edgeConstraintsOut;
 			_graphOptimizer->getConnectedGraph(fromId, poses, edgeConstraints, posesOut, edgeConstraintsOut);
-			UASSERT(edgeConstraintsOut.size() == edgeConstraints.size());
-			optimizedPoses = _graphOptimizer->optimize(fromId, posesOut, edgeConstraints, covariance, 0, error, iterationsDone);
+			optimizedPoses = _graphOptimizer->optimize(fromId, posesOut, edgeConstraintsOut, covariance, 0, error, iterationsDone);
+			if(constraints)
+			{
+				*constraints = edgeConstraintsOut;
+			}
 		}
 		else
 		{
-			// use input guess poses
+			UDEBUG("use input guess poses");
 			optimizedPoses = _graphOptimizer->optimize(fromId, poses, edgeConstraints, covariance, 0, error, iterationsDone);
+			if(constraints)
+			{
+				*constraints = edgeConstraints;
+			}
 		}
 
 		if(!poses.empty() && optimizedPoses.empty())
diff --git a/corelib/src/optimizer/OptimizerG2O.cpp b/corelib/src/optimizer/OptimizerG2O.cpp
index 97436381..b965d300 100644
--- a/corelib/src/optimizer/OptimizerG2O.cpp
+++ b/corelib/src/optimizer/OptimizerG2O.cpp
@@ -341,28 +341,23 @@ std::map<int, Transform> OptimizerG2O::optimize(
 				{
 					// check if it is SE2 or only PointXY
 					std::multimap<int, Link>::const_iterator jter=edgeConstraints.find(id);
-					if(jter != edgeConstraints.end())
+					UASSERT(jter != edgeConstraints.end());
+
+					if (1 / static_cast<double>(jter->second.infMatrix().at<double>(5,5)) >= 9999.0)
 					{
-						if (1 / static_cast<double>(jter->second.infMatrix().at<double>(5,5)) >= 9999.0)
-						{
-							g2o::VertexPointXY * v2 = new g2o::VertexPointXY();
-							v2->setEstimate(Eigen::Vector2d(iter->second.x(), iter->second.y()));
-							vertex = v2;
-							isLandmarkWithRotation.insert(std::make_pair(id, false));
-							id = landmarkVertexOffset - id;
-						}
-						else
-						{
-							g2o::VertexSE2 * v2 = new g2o::VertexSE2();
-							v2->setEstimate(g2o::SE2(iter->second.x(), iter->second.y(), iter->second.theta()));
-							vertex = v2;
-							isLandmarkWithRotation.insert(std::make_pair(id, true));
-							id = landmarkVertexOffset - id;
-						}
+						g2o::VertexPointXY * v2 = new g2o::VertexPointXY();
+						v2->setEstimate(Eigen::Vector2d(iter->second.x(), iter->second.y()));
+						vertex = v2;
+						isLandmarkWithRotation.insert(std::make_pair(id, false));
+						id = landmarkVertexOffset - id;
 					}
 					else
 					{
-						continue;
+						g2o::VertexSE2 * v2 = new g2o::VertexSE2();
+						v2->setEstimate(g2o::SE2(iter->second.x(), iter->second.y(), iter->second.theta()));
+						vertex = v2;
+						isLandmarkWithRotation.insert(std::make_pair(id, true));
+						id = landmarkVertexOffset - id;
 					}
 				}
 				else
@@ -391,34 +386,29 @@ std::map<int, Transform> OptimizerG2O::optimize(
 				{
 					// check if it is SE3 or only PointXYZ
 					std::multimap<int, Link>::const_iterator jter=edgeConstraints.find(id);
-					if(jter != edgeConstraints.end())
+					UASSERT(jter != edgeConstraints.end());
+
+					if (1 / static_cast<double>(jter->second.infMatrix().at<double>(3,3)) >= 9999.0 ||
+						1 / static_cast<double>(jter->second.infMatrix().at<double>(4,4)) >= 9999.0 ||
+						1 / static_cast<double>(jter->second.infMatrix().at<double>(5,5)) >= 9999.0)
 					{
-						if (1 / static_cast<double>(jter->second.infMatrix().at<double>(3,3)) >= 9999.0 ||
-							1 / static_cast<double>(jter->second.infMatrix().at<double>(4,4)) >= 9999.0 ||
-							1 / static_cast<double>(jter->second.infMatrix().at<double>(5,5)) >= 9999.0)
-						{
-							g2o::VertexPointXYZ * v3 = new g2o::VertexPointXYZ();
-							v3->setEstimate(Eigen::Vector3d(iter->second.x(), iter->second.y(), iter->second.z()));
-							vertex = v3;
-							isLandmarkWithRotation.insert(std::make_pair(id, false));
-							id = landmarkVertexOffset - id;
-						}
-						else
-						{
-							g2o::VertexSE3 * v3 = new g2o::VertexSE3();
-							Eigen::Affine3d a = iter->second.toEigen3d();
-							Eigen::Isometry3d pose;
-							pose = a.linear();
-							pose.translation() = a.translation();
-							v3->setEstimate(pose);
-							vertex = v3;
-							isLandmarkWithRotation.insert(std::make_pair(id, true));
-							id = landmarkVertexOffset - id;
-						}
+						g2o::VertexPointXYZ * v3 = new g2o::VertexPointXYZ();
+						v3->setEstimate(Eigen::Vector3d(iter->second.x(), iter->second.y(), iter->second.z()));
+						vertex = v3;
+						isLandmarkWithRotation.insert(std::make_pair(id, false));
+						id = landmarkVertexOffset - id;
 					}
 					else
 					{
-						continue;
+						g2o::VertexSE3 * v3 = new g2o::VertexSE3();
+						Eigen::Affine3d a = iter->second.toEigen3d();
+						Eigen::Isometry3d pose;
+						pose = a.linear();
+						pose.translation() = a.translation();
+						v3->setEstimate(pose);
+						vertex = v3;
+						isLandmarkWithRotation.insert(std::make_pair(id, true));
+						id = landmarkVertexOffset - id;
 					}
 				}
 				else
diff --git a/corelib/src/optimizer/OptimizerGTSAM.cpp b/corelib/src/optimizer/OptimizerGTSAM.cpp
index 3a0eccc8..9904c092 100644
--- a/corelib/src/optimizer/OptimizerGTSAM.cpp
+++ b/corelib/src/optimizer/OptimizerGTSAM.cpp
@@ -168,18 +168,17 @@ std::map<int, Transform> OptimizerGTSAM::optimize(
 				{
 					// check if it is SE2 or only PointXY
 					std::multimap<int, Link>::const_iterator jter=edgeConstraints.find(iter->first);
-					if(jter != edgeConstraints.end())
+					UASSERT_MSG(jter != edgeConstraints.end(), uFormat("Not found landmark %d in edges!", iter->first).c_str());
+
+					if (1 / static_cast<double>(jter->second.infMatrix().at<double>(5,5)) >= 9999.0)
 					{
-						if (1 / static_cast<double>(jter->second.infMatrix().at<double>(5,5)) >= 9999.0)
-						{
-							initialEstimate.insert(iter->first, gtsam::Point2(iter->second.x(), iter->second.y()));
-							isLandmarkWithRotation.insert(std::make_pair(iter->first, false));
-						}
-						else
-						{
-							initialEstimate.insert(iter->first, gtsam::Pose2(iter->second.x(), iter->second.y(), iter->second.theta()));
-							isLandmarkWithRotation.insert(std::make_pair(iter->first, true));
-						}
+						initialEstimate.insert(iter->first, gtsam::Point2(iter->second.x(), iter->second.y()));
+						isLandmarkWithRotation.insert(std::make_pair(iter->first, false));
+					}
+					else
+					{
+						initialEstimate.insert(iter->first, gtsam::Pose2(iter->second.x(), iter->second.y(), iter->second.theta()));
+						isLandmarkWithRotation.insert(std::make_pair(iter->first, true));
 					}
 				}
 
@@ -194,20 +193,19 @@ std::map<int, Transform> OptimizerGTSAM::optimize(
 				{
 					// check if it is SE3 or only PointXYZ
 					std::multimap<int, Link>::const_iterator jter=edgeConstraints.find(iter->first);
-					if(jter != edgeConstraints.end())
+					UASSERT_MSG(jter != edgeConstraints.end(), uFormat("Not found landmark %d in edges!", iter->first).c_str());
+
+					if (1 / static_cast<double>(jter->second.infMatrix().at<double>(3,3)) >= 9999.0 ||
+						1 / static_cast<double>(jter->second.infMatrix().at<double>(4,4)) >= 9999.0 ||
+						1 / static_cast<double>(jter->second.infMatrix().at<double>(5,5)) >= 9999.0)
 					{
-						if (1 / static_cast<double>(jter->second.infMatrix().at<double>(3,3)) >= 9999.0 ||
-							1 / static_cast<double>(jter->second.infMatrix().at<double>(4,4)) >= 9999.0 ||
-							1 / static_cast<double>(jter->second.infMatrix().at<double>(5,5)) >= 9999.0)
-						{
-							initialEstimate.insert(iter->first, gtsam::Point3(iter->second.x(), iter->second.y(), iter->second.z()));
-							isLandmarkWithRotation.insert(std::make_pair(iter->first, false));
-						}
-						else
-						{
-							initialEstimate.insert(iter->first, gtsam::Pose3(iter->second.toEigen4d()));
-							isLandmarkWithRotation.insert(std::make_pair(iter->first, true));
-						}
+						initialEstimate.insert(iter->first, gtsam::Point3(iter->second.x(), iter->second.y(), iter->second.z()));
+						isLandmarkWithRotation.insert(std::make_pair(iter->first, false));
+					}
+					else
+					{
+						initialEstimate.insert(iter->first, gtsam::Pose3(iter->second.toEigen4d()));
+						isLandmarkWithRotation.insert(std::make_pair(iter->first, true));
 					}
 				}
 			}
diff --git a/guilib/include/rtabmap/gui/PreferencesDialog.h b/guilib/include/rtabmap/gui/PreferencesDialog.h
index 0630c3a3..aaa5b210 100644
--- a/guilib/include/rtabmap/gui/PreferencesDialog.h
+++ b/guilib/include/rtabmap/gui/PreferencesDialog.h
@@ -164,6 +164,7 @@ public:
 	bool isGraphsShown() const;
 	bool isLabelsShown() const;
 	bool isLandmarksShown() const;
+	double landmarkVisSize() const;
 	bool isMarkerDetection() const;
 	double getMarkerLength() const;
 	double getVoxel() const;
diff --git a/guilib/src/DatabaseViewer.cpp b/guilib/src/DatabaseViewer.cpp
index edfee864..cd85993e 100644
--- a/guilib/src/DatabaseViewer.cpp
+++ b/guilib/src/DatabaseViewer.cpp
@@ -370,7 +370,6 @@ DatabaseViewer::DatabaseViewer(const QString & ini, QWidget * parent) :
 	connect(ui_->checkBox_ignoreLocalLoopSpace, SIGNAL(stateChanged(int)), this, SLOT(updateGraphView()));
 	connect(ui_->checkBox_ignoreLocalLoopTime, SIGNAL(stateChanged(int)), this, SLOT(updateGraphView()));
 	connect(ui_->checkBox_ignoreUserLoop, SIGNAL(stateChanged(int)), this, SLOT(updateGraphView()));
-	connect(ui_->spinBox_optimizationDepth, SIGNAL(editingFinished()), this, SLOT(updateGraphView()));
 	connect(ui_->doubleSpinBox_optimizationScale, SIGNAL(editingFinished()), this, SLOT(updateGraphView()));
 	connect(ui_->checkBox_octomap, SIGNAL(stateChanged(int)), this, SLOT(updateGrid()));
 	connect(ui_->checkBox_grid_2d, SIGNAL(stateChanged(int)), this, SLOT(updateGrid()));
@@ -698,7 +697,6 @@ void DatabaseViewer::restoreDefaultSettings()
 	ui_->checkBox_ignoreLocalLoopSpace->setChecked(false);
 	ui_->checkBox_ignoreLocalLoopTime->setChecked(false);
 	ui_->checkBox_ignoreUserLoop->setChecked(false);
-	ui_->spinBox_optimizationDepth->setValue(0);
 	ui_->doubleSpinBox_optimizationScale->setValue(1.0);
 	ui_->doubleSpinBox_gainCompensationRadius->setValue(0.0);
 	ui_->doubleSpinBox_voxelSize->setValue(0.0);
@@ -1600,7 +1598,7 @@ void DatabaseViewer::updateIds()
 	QApplication::processEvents();
 
 	std::multimap<int, Link> unilinks;
-	dbDriver_->getAllLinks(unilinks, true);
+	dbDriver_->getAllLinks(unilinks, true, true);
 	UDEBUG("%d total links loaded", (int)unilinks.size());
 	// add both direction links
 	std::multimap<int, Link> links;
@@ -1694,7 +1692,7 @@ void DatabaseViewer::updateIds()
 			std::multimap<int, Link>::iterator invertedLinkIter = graph::findLink(links, jter->second.to(), jter->second.from(), false);
 			if(	jter->second.isValid() && // null transform means a rehearsed location
 				ids.find(jter->second.from()) != ids.end() &&
-				ids.find(jter->second.to()) != ids.end() &&
+				(ids.find(jter->second.to()) != ids.end() || jter->second.to()<0) && // to add landmark links
 				graph::findLink(links_, jter->second.from(), jter->second.to()) == links_.end() &&
 				invertedLinkIter != links.end())
 			{
@@ -4995,14 +4993,15 @@ void DatabaseViewer::updateConstraintView(
 	ui_->checkBox_showOptimized->setEnabled(false);
 	UASSERT(!t.isNull() && dbDriver_);
 
-	ui_->label_type->setText(tr("%1 (%2)")
-			.arg(link.type())
+	ui_->label_type->setText(QString::number(link.type()));
+	ui_->label_type_name->setText(tr("(%1)")
 			.arg(link.type()==Link::kNeighbor?"Neighbor":
 				 link.type()==Link::kNeighborMerged?"Merged neighbor":
 				 link.type()==Link::kGlobalClosure?"Loop closure":
 				 link.type()==Link::kLocalSpaceClosure?"Space proximity link":
 				 link.type()==Link::kLocalTimeClosure?"Time proximity link":
 				 link.type()==Link::kUserClosure?"User link":
+				 link.type()==Link::kLandmark?"Landmark link":
 				 link.type()==Link::kVirtualClosure?"Virtual link":"Undefined"));
 	ui_->label_variance->setText(QString("%1, %2")
 			.arg(sqrt(link.transVariance()))
@@ -5040,45 +5039,51 @@ void DatabaseViewer::updateConstraintView(
 	{
 		ui_->horizontalSlider_A->blockSignals(true);
 		ui_->horizontalSlider_B->blockSignals(true);
-		// set from on left and to on right		{
-		ui_->horizontalSlider_A->setValue(idToIndex_.value(link.from()));
-		ui_->horizontalSlider_B->setValue(idToIndex_.value(link.to()));
+		// set from on left and to on right
+		if(link.from()>0)
+			ui_->horizontalSlider_A->setValue(idToIndex_.value(link.from()));
+		if(link.to() > 0)
+			ui_->horizontalSlider_B->setValue(idToIndex_.value(link.to()));
 		ui_->horizontalSlider_A->blockSignals(false);
 		ui_->horizontalSlider_B->blockSignals(false);
-		this->update(idToIndex_.value(link.from()),
-					ui_->label_indexA,
-					ui_->label_parentsA,
-					ui_->label_childrenA,
-					ui_->label_weightA,
-					ui_->label_labelA,
-					ui_->label_stampA,
-					ui_->graphicsView_A,
-					ui_->label_idA,
-					ui_->label_mapA,
-					ui_->label_poseA,
-					ui_->label_velA,
-					ui_->label_calibA,
-					ui_->label_scanA,
-					ui_->label_gpsA,
-					ui_->label_sensorsA,
-					false); // don't update constraints view!
-		this->update(idToIndex_.value(link.to()),
-					ui_->label_indexB,
-					ui_->label_parentsB,
-					ui_->label_childrenB,
-					ui_->label_weightB,
-					ui_->label_labelB,
-					ui_->label_stampB,
-					ui_->graphicsView_B,
-					ui_->label_idB,
-					ui_->label_mapB,
-					ui_->label_poseB,
-					ui_->label_velB,
-					ui_->label_calibB,
-					ui_->label_scanB,
-					ui_->label_gpsB,
-					ui_->label_sensorsB,
-					false); // don't update constraints view!
+		if(link.from()>0)
+			this->update(idToIndex_.value(link.from()),
+						ui_->label_indexA,
+						ui_->label_parentsA,
+						ui_->label_childrenA,
+						ui_->label_weightA,
+						ui_->label_labelA,
+						ui_->label_stampA,
+						ui_->graphicsView_A,
+						ui_->label_idA,
+						ui_->label_mapA,
+						ui_->label_poseA,
+						ui_->label_velA,
+						ui_->label_calibA,
+						ui_->label_scanA,
+						ui_->label_gpsA,
+						ui_->label_sensorsA,
+						false); // don't update constraints view!
+		if(link.to()>0)
+		{
+			this->update(idToIndex_.value(link.to()),
+						ui_->label_indexB,
+						ui_->label_parentsB,
+						ui_->label_childrenB,
+						ui_->label_weightB,
+						ui_->label_labelB,
+						ui_->label_stampB,
+						ui_->graphicsView_B,
+						ui_->label_idB,
+						ui_->label_mapB,
+						ui_->label_poseB,
+						ui_->label_velB,
+						ui_->label_calibB,
+						ui_->label_scanB,
+						ui_->label_gpsB,
+						ui_->label_sensorsB,
+						false); // don't update constraints view!
+		}
 	}
 
 	if(constraintsViewer_->isVisible())
@@ -5989,8 +5994,7 @@ void DatabaseViewer::updateGraphView()
 			int currentMapId = mapIds_.at(fromId);
 			for(std::map<int, rtabmap::Transform>::iterator iter=poses.begin(); iter!=poses.end();)
 			{
-				if(!uContains(mapIds_, iter->first) ||
-					mapIds_.at(iter->first) != currentMapId)
+				if(iter->first>0 && (!uContains(mapIds_, iter->first) || mapIds_.at(iter->first) != currentMapId))
 				{
 					poses.erase(iter++);
 				}
@@ -6011,10 +6015,8 @@ void DatabaseViewer::updateGraphView()
 			int currentMapId = mapIds_.at(fromId);
 			for(std::multimap<int, rtabmap::Link>::iterator iter=links.begin(); iter!=links.end();)
 			{
-				if(!uContains(mapIds_, iter->second.from()) ||
-					!uContains(mapIds_, iter->second.to()) ||
-					mapIds_.at(iter->second.from()) != currentMapId ||
-					mapIds_.at(iter->second.to()) != currentMapId)
+				if((iter->second.from()>0 && (!uContains(mapIds_, iter->second.from()) || mapIds_.at(iter->second.from()) != currentMapId)) ||
+					(iter->second.to()>0 && (!uContains(mapIds_, iter->second.to()) || mapIds_.at(iter->second.to()) != currentMapId)))
 				{
 					links.erase(iter++);
 				}
@@ -6056,6 +6058,7 @@ void DatabaseViewer::updateGraphView()
 		int totalLocalSpace = 0;
 		int totalUser = 0;
 		int totalPriors = 0;
+		int totalLandmarks = 0;
 		for(std::multimap<int, rtabmap::Link>::iterator iter=links.begin(); iter!=links.end();)
 		{
 			if(iter->second.type() == Link::kNeighbor)
@@ -6106,6 +6109,16 @@ void DatabaseViewer::updateGraphView()
 				loopLinks_.push_back(iter->second);
 				++totalUser;
 			}
+			else if(iter->second.type() == Link::kLandmark)
+			{
+				UASSERT(iter->second.from() > 0 && iter->second.to() < 0);
+				if(poses.find(iter->second.from()) != poses.end() && poses.find(iter->second.to()) == poses.end())
+				{
+					poses.insert(std::make_pair(iter->second.to(), poses.at(iter->second.from())*iter->second.transform()));
+				}
+				loopLinks_.push_back(iter->second);
+				++totalLandmarks;
+			}
 			else if(iter->second.type() == Link::kPosePrior)
 			{
 				++totalPriors;
@@ -6118,14 +6131,15 @@ void DatabaseViewer::updateGraphView()
 		}
 		updateLoopClosuresSlider();
 
-		ui_->label_loopClosures->setText(tr("(%1, %2, %3, %4, %5, %6, %7)")
+		ui_->label_loopClosures->setText(tr("(%1, %2, %3, %4, %5, %6, %7, %8)")
 				.arg(totalNeighbor)
 				.arg(totalNeighborMerged)
 				.arg(totalGlobal)
 				.arg(totalLocalSpace)
 				.arg(totalLocalTime)
 				.arg(totalUser)
-				.arg(totalPriors));
+				.arg(totalPriors)
+				.arg(totalLandmarks));
 
 		// remove intermediate nodes?
 		if(ui_->checkBox_ignoreIntermediateNodes->isVisible() &&
@@ -6170,8 +6184,7 @@ void DatabaseViewer::updateGraphView()
 				poses,
 				links,
 				posesOut,
-				linksOut,
-				ui_->spinBox_optimizationDepth->value());
+				linksOut);
 		if(optimizedGraphGuess.size() == posesOut.size())
 		{
 			bool identical=true;
diff --git a/guilib/src/MainWindow.cpp b/guilib/src/MainWindow.cpp
index a4efa327..10eab73d 100644
--- a/guilib/src/MainWindow.cpp
+++ b/guilib/src/MainWindow.cpp
@@ -2646,7 +2646,7 @@ void MainWindow::updateMapCloud(
 		for(std::map<int, Transform>::const_iterator iter=posesIn.begin(); iter!=posesIn.end() && iter->first<0; ++iter)
 		{
 #if PCL_VERSION_COMPARE(>=, 1, 7, 2)
-			_cloudViewer->addOrUpdateCoordinate(uFormat("landmark_%d", -iter->first), iter->second, _preferencesDialog->getMarkerLength()<=0?0.1:_preferencesDialog->getMarkerLength()/2.0, false);
+			_cloudViewer->addOrUpdateCoordinate(uFormat("landmark_%d", -iter->first), iter->second, _preferencesDialog->landmarkVisSize()>0.0?_preferencesDialog->landmarkVisSize():_preferencesDialog->getMarkerLength()<=0?0.1:_preferencesDialog->getMarkerLength()/2.0, false);
 #endif
 			if(_preferencesDialog->isLabelsShown())
 			{
diff --git a/guilib/src/PreferencesDialog.cpp b/guilib/src/PreferencesDialog.cpp
index 6698912e..624ad465 100644
--- a/guilib/src/PreferencesDialog.cpp
+++ b/guilib/src/PreferencesDialog.cpp
@@ -502,6 +502,7 @@ PreferencesDialog::PreferencesDialog(QWidget * parent) :
 	connect(_ui->checkBox_showGraphs, SIGNAL(stateChanged(int)), this, SLOT(makeObsoleteCloudRenderingPanel()));
 	connect(_ui->checkBox_showLabels, SIGNAL(stateChanged(int)), this, SLOT(makeObsoleteCloudRenderingPanel()));
 	connect(_ui->checkBox_showLandmarks, SIGNAL(stateChanged(int)), this, SLOT(makeObsoleteCloudRenderingPanel()));
+	connect(_ui->doubleSpinBox_landmarkSize, SIGNAL(valueChanged(double)), this, SLOT(makeObsoleteCloudRenderingPanel()));
 
 	connect(_ui->radioButton_noFiltering, SIGNAL(toggled(bool)), this, SLOT(makeObsoleteCloudRenderingPanel()));
 	connect(_ui->radioButton_nodeFiltering, SIGNAL(toggled(bool)), this, SLOT(makeObsoleteCloudRenderingPanel()));
@@ -1611,6 +1612,7 @@ void PreferencesDialog::resetSettings(QGroupBox * groupBox)
 		_ui->checkBox_showGraphs->setChecked(true);
 		_ui->checkBox_showLabels->setChecked(false);
 		_ui->checkBox_showLandmarks->setChecked(true);
+		_ui->doubleSpinBox_landmarkSize->setValue(0);
 
 		_ui->doubleSpinBox_mesh_angleTolerance->setValue(15.0);
 		_ui->groupBox_organized->setChecked(false);
@@ -2028,6 +2030,7 @@ void PreferencesDialog::readGuiSettings(const QString & filePath)
 	_ui->checkBox_showGraphs->setChecked(settings.value("showGraphs", _ui->checkBox_showGraphs->isChecked()).toBool());
 	_ui->checkBox_showLabels->setChecked(settings.value("showLabels", _ui->checkBox_showLabels->isChecked()).toBool());
 	_ui->checkBox_showLandmarks->setChecked(settings.value("showLandmarks", _ui->checkBox_showLandmarks->isChecked()).toBool());
+	_ui->doubleSpinBox_landmarkSize->setValue(settings.value("landmarkSize", _ui->doubleSpinBox_landmarkSize->value()).toDouble());
 
 	_ui->radioButton_noFiltering->setChecked(settings.value("noFiltering", _ui->radioButton_noFiltering->isChecked()).toBool());
 	_ui->radioButton_nodeFiltering->setChecked(settings.value("cloudFiltering", _ui->radioButton_nodeFiltering->isChecked()).toBool());
@@ -2449,6 +2452,7 @@ void PreferencesDialog::writeGuiSettings(const QString & filePath) const
 	settings.setValue("showGraphs", _ui->checkBox_showGraphs->isChecked());
 	settings.setValue("showLabels", _ui->checkBox_showLabels->isChecked());
 	settings.setValue("showLandmarks", _ui->checkBox_showLandmarks->isChecked());
+	settings.setValue("landmarkSize", _ui->doubleSpinBox_landmarkSize->value());
 
 
 	settings.setValue("noFiltering",             _ui->radioButton_noFiltering->isChecked());
@@ -4687,6 +4691,10 @@ bool PreferencesDialog::isLandmarksShown() const
 {
 	return _ui->checkBox_showLandmarks->isChecked();
 }
+double PreferencesDialog::landmarkVisSize() const
+{
+	return _ui->doubleSpinBox_landmarkSize->value();
+}
 bool PreferencesDialog::isMarkerDetection() const
 {
 	return _ui->RGBDMarkerDetection->isChecked();
diff --git a/guilib/src/ui/DatabaseViewer.ui b/guilib/src/ui/DatabaseViewer.ui
index 81210ff4..d1b11663 100644
--- a/guilib/src/ui/DatabaseViewer.ui
+++ b/guilib/src/ui/DatabaseViewer.ui
@@ -61,7 +61,7 @@
           <rect>
            <x>0</x>
            <y>0</y>
-           <width>413</width>
+           <width>409</width>
            <height>288</height>
           </rect>
          </property>
@@ -287,7 +287,7 @@
           <rect>
            <x>0</x>
            <y>0</y>
-           <width>412</width>
+           <width>408</width>
            <height>288</height>
           </rect>
          </property>
@@ -870,16 +870,6 @@
          </property>
         </widget>
        </item>
-       <item row="2" column="1">
-        <widget class="QLabel" name="label_type">
-         <property name="text">
-          <string/>
-         </property>
-         <property name="textInteractionFlags">
-          <set>Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse</set>
-         </property>
-        </widget>
-       </item>
        <item row="3" column="0">
         <widget class="QLabel" name="label_16">
          <property name="text">
@@ -921,6 +911,30 @@
          </property>
         </widget>
        </item>
+       <item row="2" column="1">
+        <layout class="QHBoxLayout" name="horizontalLayout_14" stretch="0,1">
+         <item>
+          <widget class="QLabel" name="label_type">
+           <property name="text">
+            <string/>
+           </property>
+           <property name="textInteractionFlags">
+            <set>Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse</set>
+           </property>
+          </widget>
+         </item>
+         <item>
+          <widget class="QLabel" name="label_type_name">
+           <property name="text">
+            <string/>
+           </property>
+           <property name="textInteractionFlags">
+            <set>Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse</set>
+           </property>
+          </widget>
+         </item>
+        </layout>
+       </item>
       </layout>
      </item>
      <item>
@@ -1075,7 +1089,7 @@
        <item row="9" column="1">
         <widget class="QLabel" name="label_41">
          <property name="text">
-          <string>Links (N, NM, G, LS, LT, U, P)</string>
+          <string>Links (N, NM, G, LS, LT, U, P, LM)</string>
          </property>
         </widget>
        </item>
@@ -1294,7 +1308,7 @@
      <item>
       <widget class="QToolBox" name="toolBox">
        <property name="currentIndex">
-        <number>2</number>
+        <number>0</number>
        </property>
        <widget class="QWidget" name="page_3">
         <property name="geometry">
@@ -1302,28 +1316,31 @@
           <x>0</x>
           <y>0</y>
           <width>318</width>
-          <height>197</height>
+          <height>165</height>
          </rect>
         </property>
         <attribute name="label">
          <string>Graph optimization</string>
         </attribute>
-        <layout class="QGridLayout" name="gridLayout_7" columnstretch="0,0,1">
-         <item row="0" column="2">
-          <widget class="QLabel" name="label_8">
+        <layout class="QGridLayout" name="gridLayout_7" columnstretch="0,0,0">
+         <item row="2" column="0">
+          <widget class="QCheckBox" name="checkBox_ignoreGlobalLoop">
            <property name="text">
-            <string>Depth (0=inf)</string>
+            <string/>
+           </property>
+           <property name="checked">
+            <bool>false</bool>
            </property>
           </widget>
          </item>
-         <item row="5" column="2">
+         <item row="4" column="2">
           <widget class="QLabel" name="label_49">
            <property name="text">
             <string>Ignore local loop closures (time)</string>
            </property>
           </widget>
          </item>
-         <item row="4" column="0">
+         <item row="3" column="0">
           <widget class="QCheckBox" name="checkBox_ignoreLocalLoopSpace">
            <property name="text">
             <string/>
@@ -1333,21 +1350,21 @@
            </property>
           </widget>
          </item>
-         <item row="3" column="2">
+         <item row="2" column="2">
           <widget class="QLabel" name="label_48">
            <property name="text">
             <string>Ignore global loop closures</string>
            </property>
           </widget>
          </item>
-         <item row="2" column="2">
+         <item row="1" column="2">
           <widget class="QLabel" name="label_35">
            <property name="text">
             <string>Ignore pose correction</string>
            </property>
           </widget>
          </item>
-         <item row="2" column="0">
+         <item row="1" column="0">
           <widget class="QCheckBox" name="checkBox_ignorePoseCorrection">
            <property name="text">
             <string/>
@@ -1357,7 +1374,7 @@
            </property>
           </widget>
          </item>
-         <item row="5" column="0">
+         <item row="4" column="0">
           <widget class="QCheckBox" name="checkBox_ignoreLocalLoopTime">
            <property name="text">
             <string/>
@@ -1367,7 +1384,7 @@
            </property>
           </widget>
          </item>
-         <item row="7" column="0">
+         <item row="6" column="0">
           <spacer name="verticalSpacer_3">
            <property name="orientation">
             <enum>Qt::Vertical</enum>
@@ -1380,7 +1397,7 @@
            </property>
           </spacer>
          </item>
-         <item row="6" column="0">
+         <item row="5" column="0">
           <widget class="QCheckBox" name="checkBox_ignoreUserLoop">
            <property name="text">
             <string/>
@@ -1390,38 +1407,28 @@
            </property>
           </widget>
          </item>
-         <item row="6" column="2">
+         <item row="5" column="2">
           <widget class="QLabel" name="label_50">
            <property name="text">
             <string>Ignore user loop closures</string>
            </property>
           </widget>
          </item>
-         <item row="3" column="0">
-          <widget class="QCheckBox" name="checkBox_ignoreGlobalLoop">
-           <property name="text">
-            <string/>
-           </property>
-           <property name="checked">
-            <bool>false</bool>
-           </property>
-          </widget>
-         </item>
-         <item row="4" column="2">
+         <item row="3" column="2">
           <widget class="QLabel" name="label_47">
            <property name="text">
             <string>Ignore local loop closures (space)</string>
            </property>
           </widget>
          </item>
-         <item row="1" column="2">
+         <item row="0" column="2">
           <widget class="QLabel" name="label_scale_title">
            <property name="text">
             <string>Scale</string>
            </property>
           </widget>
          </item>
-         <item row="1" column="0">
+         <item row="0" column="0">
           <widget class="QDoubleSpinBox" name="doubleSpinBox_optimizationScale">
            <property name="decimals">
             <number>3</number>
@@ -1440,22 +1447,6 @@
            </property>
           </widget>
          </item>
-         <item row="0" column="0">
-          <widget class="QSpinBox" name="spinBox_optimizationDepth">
-           <property name="suffix">
-            <string/>
-           </property>
-           <property name="minimum">
-            <number>0</number>
-           </property>
-           <property name="maximum">
-            <number>1000</number>
-           </property>
-           <property name="value">
-            <number>0</number>
-           </property>
-          </widget>
-         </item>
         </layout>
        </widget>
        <widget class="QWidget" name="page_4">
@@ -1463,8 +1454,8 @@
          <rect>
           <x>0</x>
           <y>0</y>
-          <width>280</width>
-          <height>875</height>
+          <width>519</width>
+          <height>791</height>
          </rect>
         </property>
         <attribute name="label">
diff --git a/guilib/src/ui/preferencesDialog.ui b/guilib/src/ui/preferencesDialog.ui
index 82d02d9d..7e10328d 100644
--- a/guilib/src/ui/preferencesDialog.ui
+++ b/guilib/src/ui/preferencesDialog.ui
@@ -94,7 +94,7 @@
           <property name="geometry">
            <rect>
             <x>0</x>
-            <y>0</y>
+            <y>-1219</y>
             <width>680</width>
             <height>3082</height>
            </rect>
@@ -126,7 +126,7 @@
                 <enum>QFrame::Raised</enum>
                </property>
                <property name="currentIndex">
-                <number>21</number>
+                <number>1</number>
                </property>
                <widget class="QWidget" name="page_22">
                 <layout class="QVBoxLayout" name="verticalLayout_29" stretch="0,1">
@@ -1901,6 +1901,35 @@ Show a yellow background when the number of odometry inliers goes under this thr
                          </property>
                         </widget>
                        </item>
+                       <item row="7" column="2">
+                        <widget class="QLabel" name="label_528">
+                         <property name="text">
+                          <string>Landmark size. If zero, marker's length parameter is used.</string>
+                         </property>
+                         <property name="wordWrap">
+                          <bool>true</bool>
+                         </property>
+                         <property name="textInteractionFlags">
+                          <set>Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse</set>
+                         </property>
+                        </widget>
+                       </item>
+                       <item row="7" column="0">
+                        <widget class="QDoubleSpinBox" name="doubleSpinBox_landmarkSize">
+                         <property name="suffix">
+                          <string> m</string>
+                         </property>
+                         <property name="minimum">
+                          <double>0.000000000000000</double>
+                         </property>
+                         <property name="singleStep">
+                          <double>0.100000000000000</double>
+                         </property>
+                         <property name="value">
+                          <double>0.100000000000000</double>
+                         </property>
+                        </widget>
+                       </item>
                       </layout>
                      </widget>
                     </item>