iSAM2 integration (#1249)

* isam2 integration

* fonctional iSAM2, added related parameters

* updated default params after testing large-scale enviroment

* proximity search optimization

* boost<1.68 fix

* boost version

* Fixed map not showing in graphview

* iSAM2: supporting MM/localization/multi-session modes

corelib/include/rtabmap/core/Parameters.h

@@ -436,6 +436,9 @@ class RTABMAP_CORE_EXPORT Parameters
     RTABMAP_PARAM(g2o, Baseline,          double, 0.075,   "When doing bundle adjustment with RGB-D data, we can set a fake baseline (m) to do stereo bundle adjustment (if 0, mono bundle adjustment is done). For stereo data, the baseline in the calibration is used directly.");
 
     RTABMAP_PARAM(GTSAM, Optimizer,       int, 1,          "0=Levenberg 1=GaussNewton 2=Dogleg");
+    RTABMAP_PARAM(GTSAM, Incremental,     bool, false,     uFormat("Do graph optimization incrementally (iSAM2) to increase optimization speed on loop closures. Note that only GaussNewton and Dogleg optimization algorithms are supported (%s) in this mode.", kGTSAMOptimizer().c_str()));
+    RTABMAP_PARAM(GTSAM, IncRelinearizeThreshold,  double, 0.01, "Only relinearize variables whose linear delta magnitude is greater than this threshold. See GTSAM::ISAM2 doc for more info.");
+    RTABMAP_PARAM(GTSAM, IncRelinearizeSkip,       int, 1, "Only relinearize any variables every X calls to ISAM2::update(). See GTSAM::ISAM2 doc for more info.");
 
     // Odometry
     RTABMAP_PARAM(Odom, Strategy,               int, 0,       "0=Frame-to-Map (F2M) 1=Frame-to-Frame (F2F) 2=Fovis 3=viso2 4=DVO-SLAM 5=ORB_SLAM2 6=OKVIS 7=LOAM 8=MSCKF_VIO 9=VINS-Fusion 10=OpenVINS 11=FLOAM 12=Open3D");

corelib/include/rtabmap/core/Statistics.h

@@ -157,6 +157,7 @@ class RTABMAP_CORE_EXPORT Statistics
 	RTABMAP_STATS(Timing, Memory_update, ms);
 	RTABMAP_STATS(Timing, Neighbor_link_refining, ms);
 	RTABMAP_STATS(Timing, Proximity_by_time, ms);
+	RTABMAP_STATS(Timing, Proximity_by_space_search, ms);
 	RTABMAP_STATS(Timing, Proximity_by_space_visual, ms);
 	RTABMAP_STATS(Timing, Proximity_by_space, ms);
 	RTABMAP_STATS(Timing, Cleaning_neighbors, ms);

corelib/include/rtabmap/core/optimizer/OptimizerGTSAM.h

@@ -30,6 +30,10 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 
 #include <rtabmap/core/Optimizer.h>
 
+namespace gtsam {
+	class ISAM2;
+}
+
 namespace rtabmap {
 
 class RTABMAP_CORE_EXPORT OptimizerGTSAM : public Optimizer
@@ -38,13 +42,8 @@ public:
 	static bool available();
 
 public:
-	OptimizerGTSAM(const ParametersMap & parameters = ParametersMap()) :
+	OptimizerGTSAM(const ParametersMap & parameters = ParametersMap());
-		Optimizer(parameters),
+	virtual ~OptimizerGTSAM();
-		optimizer_(Parameters::defaultGTSAMOptimizer())
-	{
-		parseParameters(parameters);
-	}
-	virtual ~OptimizerGTSAM() {}
 
 	virtual Type type() const {return kTypeGTSAM;}
 
@@ -60,7 +59,25 @@ public:
 			int * iterationsDone = 0);
 
 private:
-	int optimizer_;
+	int internalOptimizerType_;
+
+	gtsam::ISAM2 * isam2_;
+	struct ConstraintToFactor {
+		ConstraintToFactor(int _from, int _to, std::uint64_t _factorIndice)
+		{
+			from = _from;
+			to = _to;
+			factorIndice = _factorIndice;
+		}
+		int from;
+		int to;
+		std::uint64_t factorIndice;
+	};
+
+	std::vector<ConstraintToFactor> lastAddedConstraints_;
+	int lastSwitchId_;
+	std::set<int> addedPoses_;
+	std::pair<int, std::uint64_t> lastRootFactorIndex_;
 };
 
 } /* namespace rtabmap */

corelib/src/CameraThread.cpp

@@ -292,6 +292,11 @@ bool CameraThread::odomProvided() const
 void CameraThread::mainLoopBegin()
 {
 	ULogger::registerCurrentThread("Camera");
+	if(_imuFilter)
+	{
+		// In case we paused the camera and moved somewhere else, restart filtering.
+		_imuFilter->reset();
+	}
 	_camera->resetTimer();
 }
 

corelib/src/GlobalMap.cpp

@@ -121,7 +121,7 @@ bool GlobalMap::update(const std::map<int, Transform> & poses)
 		}
 		else
 		{
-			UDEBUG("Updated pose for node %d is not found, some points may not be copied. Use negative ids to just update cell values without adding new ones.", jter->first);
+			UDEBUG("Updated pose for node %d is not found, some points may not be copied. Use negative ids to just update cell values without adding new ones.", iter->first);
 		}
 	}
 

corelib/src/Rtabmap.cpp

@@ -1094,6 +1094,13 @@ void Rtabmap::resetMemory()
 	{
 		UERROR("RTAB-Map is not initialized. No memory to reset...");
 	}
+
+	if(_graphOptimizer)
+	{
+		delete _graphOptimizer;
+		_graphOptimizer = Optimizer::create(_parameters);
+	}
+
 	this->setupLogFiles(true);
 }
 
@@ -1177,6 +1184,7 @@ bool Rtabmap::process(
 	double timeMemoryUpdate = 0;
 	double timeNeighborLinkRefining = 0;
 	double timeProximityByTimeDetection = 0;
+	double timeProximityBySpaceSearch = 0;
 	double timeProximityBySpaceVisualDetection = 0;
 	double timeProximityBySpaceDetection = 0;
 	double timeCleaningNeighbors = 0;
@@ -2598,22 +2606,39 @@ bool Rtabmap::process(
 				// 1) compare visually with nearest locations
 				//
 				UDEBUG("Proximity detection (local loop closure in SPACE using matching images, local radius=%fm)", _localRadius);
-				std::map<int, float> nearestIds;
+				std::map<int, float> nearestIds = graph::findNearestNodes(signature->id(), _optimizedPoses, _localRadius);
-				if(_memory->isIncremental() && _proximityMaxGraphDepth > 0)
-				{
-					nearestIds = _memory->getNeighborsIdRadius(signature->id(), _localRadius, _optimizedPoses, _proximityMaxGraphDepth);
-				}
-				else
-				{
-					nearestIds = graph::findNearestNodes(signature->id(), _optimizedPoses, _localRadius);
-				}
 				UDEBUG("nearestIds=%d/%d", (int)nearestIds.size(), (int)_optimizedPoses.size());
 				std::map<int, Transform> nearestPoses;
+				std::multimap<int, int> links;
+				if(_memory->isIncremental() && _proximityMaxGraphDepth>0)
+				{
+					// get bidirectional links
+					for(std::multimap<int, Link>::iterator iter=_constraints.begin(); iter!=_constraints.end(); ++iter)
+					{
+						if(uContains(_optimizedPoses, iter->second.from()) && uContains(_optimizedPoses, iter->second.to()))
+						{
+							links.insert(std::make_pair(iter->second.from(), iter->second.to()));
+							links.insert(std::make_pair(iter->second.to(), iter->second.from())); // <->
+						}
+					}
+				}
 				for(std::map<int, float>::iterator iter=nearestIds.lower_bound(1); iter!=nearestIds.end(); ++iter)
 				{
 					if(_memory->getStMem().find(iter->first) == _memory->getStMem().end())
 					{
-						nearestPoses.insert(std::make_pair(iter->first, _optimizedPoses.at(iter->first)));
+						if(_memory->isIncremental() && _proximityMaxGraphDepth > 0)
+						{
+							std::list<std::pair<int, Transform> > path = graph::computePath(_optimizedPoses, links, signature->id(), iter->first);
+							UDEBUG("Graph depth to %d = %ld", iter->first, path.size());
+							if(!path.empty() && (int)path.size() <= _proximityMaxGraphDepth)
+							{
+								nearestPoses.insert(std::make_pair(iter->first, _optimizedPoses.at(iter->first)));
+							}
+						}
+						else
+						{
+							nearestPoses.insert(std::make_pair(iter->first, _optimizedPoses.at(iter->first)));
+						}
 					}
 				}
 				UDEBUG("nearestPoses=%d", (int)nearestPoses.size());
@@ -2645,6 +2670,9 @@ bool Rtabmap::process(
 				}
 				UDEBUG("nearestPaths=%d proximityMaxPaths=%d", (int)nearestPaths.size(), _proximityMaxPaths);
 
+				timeProximityBySpaceSearch = timer.ticks();
+				ULOGGER_INFO("timeProximityBySpaceSearch=%fs", timeProximityBySpaceSearch);
+
 				float proximityFilteringRadius = _proximityFilteringRadius;
 				if(_maxLoopClosureDistance>0.0f && (proximityFilteringRadius <= 0.0f || _maxLoopClosureDistance<proximityFilteringRadius))
 				{
@@ -4072,6 +4100,7 @@ bool Rtabmap::process(
 			statistics_.addStatistic(Statistics::kTimingMemory_update(), timeMemoryUpdate*1000);
 			statistics_.addStatistic(Statistics::kTimingNeighbor_link_refining(), timeNeighborLinkRefining*1000);
 			statistics_.addStatistic(Statistics::kTimingProximity_by_time(), timeProximityByTimeDetection*1000);
+			statistics_.addStatistic(Statistics::kTimingProximity_by_space_search(), timeProximityBySpaceSearch*1000);
 			statistics_.addStatistic(Statistics::kTimingProximity_by_space_visual(), timeProximityBySpaceVisualDetection*1000);
 			statistics_.addStatistic(Statistics::kTimingProximity_by_space(), timeProximityBySpaceDetection*1000);
 			statistics_.addStatistic(Statistics::kTimingReactivation(), timeReactivations*1000);

corelib/src/optimizer/OptimizerGTSAM.cpp

@@ -53,6 +53,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 #include "gtsam/GravityFactor.h"
 #include <optimizer/gtsam/XYFactor.h>
 #include <optimizer/gtsam/XYZFactor.h>
+#include <gtsam/nonlinear/ISAM2.h>
 
 #ifdef RTABMAP_VERTIGO
 #include "vertigo/gtsam/betweenFactorSwitchable.h"
@@ -62,6 +63,23 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 
 namespace rtabmap {
 
+OptimizerGTSAM::OptimizerGTSAM(const ParametersMap & parameters) :
+	Optimizer(parameters),
+	internalOptimizerType_(Parameters::defaultGTSAMOptimizer()),
+	isam2_(0),
+	lastSwitchId_(1000000000)
+{
+	lastRootFactorIndex_.first = 0;
+	parseParameters(parameters);
+}
+
+OptimizerGTSAM::~OptimizerGTSAM()
+{
+#ifdef RTABMAP_GTSAM
+	delete isam2_;
+#endif
+}
+
 bool OptimizerGTSAM::available()
 {
 #ifdef RTABMAP_GTSAM
@@ -74,7 +92,47 @@ bool OptimizerGTSAM::available()
 void OptimizerGTSAM::parseParameters(const ParametersMap & parameters)
 {
 	Optimizer::parseParameters(parameters);
-	Parameters::parse(parameters, Parameters::kGTSAMOptimizer(), optimizer_);
+#ifdef RTABMAP_GTSAM
+	Parameters::parse(parameters, Parameters::kGTSAMOptimizer(), internalOptimizerType_);
+
+	bool incremental = isam2_;
+	double threshold = Parameters::defaultGTSAMIncRelinearizeThreshold();
+	int skip = Parameters::defaultGTSAMIncRelinearizeSkip();
+	Parameters::parse(parameters, Parameters::kGTSAMIncremental(), incremental);
+	Parameters::parse(parameters, Parameters::kGTSAMIncRelinearizeThreshold(), threshold);
+	Parameters::parse(parameters, Parameters::kGTSAMIncRelinearizeSkip(), skip);
+	UDEBUG("GTSAM %s=%d", Parameters::kGTSAMOptimizer().c_str(), internalOptimizerType_);
+	UDEBUG("GTSAM %s=%d", Parameters::kGTSAMIncremental().c_str(), incremental?1:0);
+	UDEBUG("GTSAM %s=%f", Parameters::kGTSAMIncRelinearizeThreshold().c_str(), threshold);
+	UDEBUG("GTSAM %s=%d", Parameters::kGTSAMIncRelinearizeSkip().c_str(), skip);
+	if(incremental && !isam2_)
+	{
+		gtsam::ISAM2Params::OptimizationParams optParams;
+		if(internalOptimizerType_==2)
+		{
+			optParams = gtsam::ISAM2DoglegParams();
+		}
+		else
+		{
+			optParams = gtsam::ISAM2GaussNewtonParams();
+		}
+		gtsam::ISAM2Params params(optParams);
+		params.relinearizeThreshold = threshold;
+		params.relinearizeSkip = skip;
+		params.evaluateNonlinearError = true;
+		isam2_ = new ISAM2(params);
+
+		addedPoses_.clear();
+		lastAddedConstraints_.clear();
+		lastRootFactorIndex_.first = 0;
+		lastSwitchId_ = 1000000000;
+	}
+	else if(!incremental && isam2_)
+	{
+		delete isam2_;
+		isam2_ = 0;
+	}
+#endif
 }
 
 std::map<int, Transform> OptimizerGTSAM::optimize(
@@ -138,9 +196,13 @@ std::map<int, Transform> OptimizerGTSAM::optimize(
 			}
 		}
 
+		std::vector<ConstraintToFactor> addedPrior;
+		gtsam::FactorIndices removeFactorIndices;
+
 		//prior first pose
-		if(rootId != 0)
+		if(rootId != 0 && (!isam2_ || lastRootFactorIndex_.first != rootId))
 		{
+			UDEBUG("Setting prior for rootId=%d", rootId);
 			UASSERT(uContains(poses, rootId));
 			const Transform & initialPose = poses.at(rootId);
 			UDEBUG("hasGPSPrior=%s", hasGPSPrior?"true":"false");
@@ -148,6 +210,7 @@ std::map<int, Transform> OptimizerGTSAM::optimize(
 			{
 				gtsam::noiseModel::Diagonal::shared_ptr priorNoise = gtsam::noiseModel::Diagonal::Variances(gtsam::Vector3(0.01, 0.01, hasGPSPrior?1e-2:std::numeric_limits<double>::min()));
 				graph.add(gtsam::PriorFactor<gtsam::Pose2>(rootId, gtsam::Pose2(initialPose.x(), initialPose.y(), initialPose.theta()), priorNoise));
+				addedPrior.push_back(ConstraintToFactor(rootId, rootId, -1));
 			}
 			else
 			{
@@ -157,14 +220,95 @@ std::map<int, Transform> OptimizerGTSAM::optimize(
 								(hasGPSPrior?2:1e-2), hasGPSPrior?2:1e-2, hasGPSPrior?2:1e-2 // xyz
 								).finished());
 				graph.add(gtsam::PriorFactor<gtsam::Pose3>(rootId, gtsam::Pose3(initialPose.toEigen4d()), priorNoise));
+				addedPrior.push_back(ConstraintToFactor(rootId, rootId, -1));
 			}
+			if(isam2_ && lastRootFactorIndex_.first!=0)
+			{
+				if(uContains(poses, lastRootFactorIndex_.first))
+				{
+					UDEBUG("isam2: switching rootid from %d to %d", lastRootFactorIndex_.first, rootId);
+					removeFactorIndices.push_back(lastRootFactorIndex_.second);
+				}
+				else
+				{
+					UDEBUG("isam2: reset iSAM2, disjoint mapping sessions between previous root %d and new root %d", lastRootFactorIndex_.first, rootId);
+					// reset iSAM2, disjoint mapping session
+					gtsam::ISAM2Params params = isam2_->params();
+					delete isam2_;
+					isam2_ = new gtsam::ISAM2(params);
+					addedPoses_.clear();
+					lastAddedConstraints_.clear();
+					lastRootFactorIndex_.first = 0;
+					lastSwitchId_ = 1000000000;
+				}
+				lastRootFactorIndex_.first = 0;
+			}
+		}
+
+		std::map<int, Transform> newPoses;
+		std::multimap<int, Link> newEdgeConstraints;
+
+		if(isam2_)
+		{
+			UDEBUG("Add new poses...");
+			// new poses?
+			for(std::map<int, Transform>::const_iterator iter=poses.begin(); iter!=poses.end(); ++iter)
+			{
+				if(addedPoses_.find(iter->first) == addedPoses_.end())
+				{
+					newPoses.insert(*iter);
+					UDEBUG("Adding pose %d to factor graph", iter->first);
+				}
+			}
+			UDEBUG("Add new links...");
+			// new links?
+			for(std::multimap<int, Link>::const_iterator iter=edgeConstraints.begin(); iter!=edgeConstraints.end(); ++iter)
+			{
+				if(addedPoses_.find(iter->second.from()) == addedPoses_.end() ||
+				   addedPoses_.find(iter->second.to()) == addedPoses_.end())
+				{
+					newEdgeConstraints.insert(*iter);
+					UDEBUG("Adding constraint %d (%d->%d) to factor graph", iter->first, iter->second.from(), iter->second.to());
+				}
+			}
+
+			if(!this->isRobust())
+			{
+				UDEBUG("Remove links...");
+				// Remove constraints not there anymore in case the last loop closures were rejected.
+				// As we don't track "switch" constraints, we don't support this if vertigo is used.
+				for(size_t i=0; i<lastAddedConstraints_.size(); ++i)
+				{
+					if(lastAddedConstraints_[i].from != lastAddedConstraints_[i].to &&
+					   graph::findLink(edgeConstraints, lastAddedConstraints_[i].from, lastAddedConstraints_[i].to) == edgeConstraints.end())
+					{
+						removeFactorIndices.push_back(lastAddedConstraints_[i].factorIndice);
+						UDEBUG("Removing constraint %d->%d (factor indice=%ld)",
+								lastAddedConstraints_[i].from,
+								lastAddedConstraints_[i].to,
+								lastAddedConstraints_[i].factorIndice);
+					}
+				}
+			}
+			else if(poses.rbegin()->first >= 1000000000)
+			{
+				UERROR("Lastest pose id (%d) is too huge for robust optimization (over switch offset of 1000000000)", poses.rbegin()->first);
+				return optimizedPoses;
+			}
+
+			lastAddedConstraints_ = addedPrior;
+		}
+		else
+		{
+			newPoses = poses;
+			newEdgeConstraints = edgeConstraints;
 		}
 
 		UDEBUG("fill poses to gtsam... rootId=%d (priorsIgnored=%d landmarksIgnored=%d)",
 				rootId, priorsIgnored()?1:0, landmarksIgnored()?1:0);
 		gtsam::Values initialEstimate;
 		std::map<int, bool> isLandmarkWithRotation;
-		for(std::map<int, Transform>::const_iterator iter = poses.begin(); iter!=poses.end(); ++iter)
+		for(std::map<int, Transform>::const_iterator iter = newPoses.begin(); iter!=newPoses.end(); ++iter)
 		{
 			UASSERT(!iter->second.isNull());
 			if(isSlam2d())
@@ -172,12 +316,13 @@ std::map<int, Transform> OptimizerGTSAM::optimize(
 				if(iter->first > 0)
 				{
 					initialEstimate.insert(iter->first, gtsam::Pose2(iter->second.x(), iter->second.y(), iter->second.theta()));
+					addedPoses_.insert(iter->first);
 				}
 				else if(!landmarksIgnored())
 				{
 					// check if it is SE2 or only PointXY
-					std::multimap<int, Link>::const_iterator jter=edgeConstraints.find(iter->first);
+					std::multimap<int, Link>::const_iterator jter=newEdgeConstraints.find(iter->first);
-					UASSERT_MSG(jter != edgeConstraints.end(), uFormat("Not found landmark %d in edges!", iter->first).c_str());
+					UASSERT_MSG(jter != newEdgeConstraints.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)
 					{
@@ -189,6 +334,7 @@ std::map<int, Transform> OptimizerGTSAM::optimize(
 						initialEstimate.insert(iter->first, gtsam::Pose2(iter->second.x(), iter->second.y(), iter->second.theta()));
 						isLandmarkWithRotation.insert(std::make_pair(iter->first, true));
 					}
+					addedPoses_.insert(iter->first);
 				}
 
 			}
@@ -197,12 +343,13 @@ std::map<int, Transform> OptimizerGTSAM::optimize(
 				if(iter->first > 0)
 				{
 					initialEstimate.insert(iter->first, gtsam::Pose3(iter->second.toEigen4d()));
+					addedPoses_.insert(iter->first);
 				}
 				else if(!landmarksIgnored())
 				{
 					// check if it is SE3 or only PointXYZ
-					std::multimap<int, Link>::const_iterator jter=edgeConstraints.find(iter->first);
+					std::multimap<int, Link>::const_iterator jter=newEdgeConstraints.find(iter->first);
-					UASSERT_MSG(jter != edgeConstraints.end(), uFormat("Not found landmark %d in edges!", iter->first).c_str());
+					UASSERT_MSG(jter != newEdgeConstraints.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 ||
@@ -216,19 +363,23 @@ std::map<int, Transform> OptimizerGTSAM::optimize(
 						initialEstimate.insert(iter->first, gtsam::Pose3(iter->second.toEigen4d()));
 						isLandmarkWithRotation.insert(std::make_pair(iter->first, true));
 					}
+					addedPoses_.insert(iter->first);
 				}
 			}
 		}
 
 		UDEBUG("fill edges to gtsam...");
-		int switchCounter = poses.rbegin()->first+1;
+		if(!isam2_)
-		for(std::multimap<int, Link>::const_iterator iter=edgeConstraints.begin(); iter!=edgeConstraints.end(); ++iter)
+		{
+			lastSwitchId_ = newPoses.rbegin()->first+1;
+		}
+		for(std::multimap<int, Link>::const_iterator iter=newEdgeConstraints.begin(); iter!=newEdgeConstraints.end(); ++iter)
 		{
 			int id1 = iter->second.from();
 			int id2 = iter->second.to();
 
-            UASSERT_MSG(initialEstimate.find(id1)!=initialEstimate.end(), uFormat("id1=%d", id1).c_str());
+            UASSERT_MSG(poses.find(id1)!=poses.end(), uFormat("id1=%d", id1).c_str());
-            UASSERT_MSG(initialEstimate.find(id2)!=initialEstimate.end(), uFormat("id2=%d", id2).c_str());
+            UASSERT_MSG(poses.find(id2)!=poses.end(), uFormat("id2=%d", id2).c_str());
 
 			UASSERT(!iter->second.transform().isNull());
 			if(id1 == id2)
@@ -244,6 +395,7 @@ std::map<int, Transform> OptimizerGTSAM::optimize(
 									1/iter->second.infMatrix().at<double>(0,0),
 									1/iter->second.infMatrix().at<double>(1,1)));
 							graph.add(XYFactor<gtsam::Point2>(id1, gtsam::Point2(iter->second.transform().x(), iter->second.transform().y()), model));
+							lastAddedConstraints_.push_back(ConstraintToFactor(id1, id1, -1));
 						}
 						else if (1 / static_cast<double>(iter->second.infMatrix().at<double>(5,5)) >= 9999.0)
 						{
@@ -251,6 +403,7 @@ std::map<int, Transform> OptimizerGTSAM::optimize(
 									1/iter->second.infMatrix().at<double>(0,0),
 									1/iter->second.infMatrix().at<double>(1,1)));
 							graph.add(XYFactor<gtsam::Pose2>(id1, gtsam::Point2(iter->second.transform().x(), iter->second.transform().y()), model));
+							lastAddedConstraints_.push_back(ConstraintToFactor(id1, id1, -1));
 						}
 						else
 						{
@@ -270,6 +423,7 @@ std::map<int, Transform> OptimizerGTSAM::optimize(
 
 							gtsam::noiseModel::Gaussian::shared_ptr model = gtsam::noiseModel::Gaussian::Information(information);
 							graph.add(gtsam::PriorFactor<gtsam::Pose2>(id1, gtsam::Pose2(iter->second.transform().x(), iter->second.transform().y(), iter->second.transform().theta()), model));
+							lastAddedConstraints_.push_back(ConstraintToFactor(id1, id1, -1));
 						}
 					}
 					else
@@ -281,6 +435,7 @@ std::map<int, Transform> OptimizerGTSAM::optimize(
 										iter->second.infMatrix().at<double>(1,1),
 										iter->second.infMatrix().at<double>(2,2)));
 							graph.add(XYZFactor<gtsam::Point3>(id1, gtsam::Point3(iter->second.transform().x(), iter->second.transform().y(), iter->second.transform().z()), model));
+							lastAddedConstraints_.push_back(ConstraintToFactor(id1, id1, -1));
 						}
 						else if (1 / static_cast<double>(iter->second.infMatrix().at<double>(3,3)) >= 9999.0 ||
 							1 / static_cast<double>(iter->second.infMatrix().at<double>(4,4)) >= 9999.0 ||
@@ -291,6 +446,7 @@ std::map<int, Transform> OptimizerGTSAM::optimize(
 										iter->second.infMatrix().at<double>(1,1),
 										iter->second.infMatrix().at<double>(2,2)));
 							graph.add(XYZFactor<gtsam::Pose3>(id1, gtsam::Point3(iter->second.transform().x(), iter->second.transform().y(), iter->second.transform().z()), model));
+							lastAddedConstraints_.push_back(ConstraintToFactor(id1, id1, -1));
 						}
 						else
 						{
@@ -308,15 +464,17 @@ std::map<int, Transform> OptimizerGTSAM::optimize(
 							gtsam::SharedNoiseModel model = gtsam::noiseModel::Gaussian::Information(mgtsam);
 
 							graph.add(gtsam::PriorFactor<gtsam::Pose3>(id1, gtsam::Pose3(iter->second.transform().toEigen4d()), model));
+							lastAddedConstraints_.push_back(ConstraintToFactor(id1, id1, -1));
 						}
 					}
 				}
-				else if(!isSlam2d() && gravitySigma() > 0 && iter->second.type() == Link::kGravity && poses.find(iter->first) != poses.end())
+				else if(!isSlam2d() && gravitySigma() > 0 && iter->second.type() == Link::kGravity && newPoses.find(iter->first) != newPoses.end())
 				{
 					Vector3 r = gtsam::Pose3(iter->second.transform().toEigen4d()).rotation().xyz();
 					gtsam::Unit3 nG = gtsam::Rot3::RzRyRx(r.x(), r.y(), 0).rotate(gtsam::Unit3(0,0,-1));
 					gtsam::SharedNoiseModel model = gtsam::noiseModel::Isotropic::Sigmas(gtsam::Vector2(gravitySigma(), 10));
 					graph.add(Pose3GravityFactor(iter->first, nG, model, Unit3(0,0,1)));
+					lastAddedConstraints_.push_back(ConstraintToFactor(iter->first, iter->first, -1));
 				}
 			}
 			else if(id1<0 || id2 < 0)
@@ -354,6 +512,7 @@ std::map<int, Transform> OptimizerGTSAM::optimize(
 							}
 							gtsam::noiseModel::Gaussian::shared_ptr model = gtsam::noiseModel::Gaussian::Information(information);
 							graph.add(gtsam::BetweenFactor<gtsam::Pose2>(id1, id2, gtsam::Pose2(t.x(), t.y(), t.theta()), model));
+							lastAddedConstraints_.push_back(ConstraintToFactor(id1, id2, -1));
 						}
 						else
 						{
@@ -368,6 +527,7 @@ std::map<int, Transform> OptimizerGTSAM::optimize(
 							gtsam::Point2 landmark(t.x(), t.y());
 							gtsam::Pose2 p;
 							graph.add(gtsam::BearingRangeFactor<gtsam::Pose2, gtsam::Point2>(id1, id2, p.bearing(landmark), p.range(landmark), model));
+							lastAddedConstraints_.push_back(ConstraintToFactor(id1, id2, -1));
 						}
 					}
 					else
@@ -387,6 +547,7 @@ std::map<int, Transform> OptimizerGTSAM::optimize(
 							mgtsam.block(3,0,3,3) = information.block(0,3,3,3); // off diagonal
 							gtsam::SharedNoiseModel model = gtsam::noiseModel::Gaussian::Information(mgtsam);
 							graph.add(gtsam::BetweenFactor<gtsam::Pose3>(id1, id2, gtsam::Pose3(t.toEigen4d()), model));
+							lastAddedConstraints_.push_back(ConstraintToFactor(id1, id2, -1));
 						}
 						else
 						{
@@ -401,6 +562,7 @@ std::map<int, Transform> OptimizerGTSAM::optimize(
 							gtsam::Point3 landmark(t.x(), t.y(), t.z());
 							gtsam::Pose3 p;
 							graph.add(gtsam::BearingRangeFactor<gtsam::Pose3, gtsam::Point3>(id1, id2, p.bearing(landmark), p.range(landmark), model));
+							lastAddedConstraints_.push_back(ConstraintToFactor(id1, id2, -1));
 						}
 					}
 				}
@@ -418,7 +580,7 @@ std::map<int, Transform> OptimizerGTSAM::optimize(
 					//  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));
+					initialEstimate.insert(gtsam::Symbol('s',lastSwitchId_), vertigo::SwitchVariableLinear(prior));
 
 					// create switch prior factor
 					// "If the front-end is not able to assign sound individual values
@@ -426,7 +588,7 @@ std::map<int, Transform> OptimizerGTSAM::optimize(
 					//  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));
+					graph.add(gtsam::PriorFactor<vertigo::SwitchVariableLinear> (gtsam::Symbol('s',lastSwitchId_), vertigo::SwitchVariableLinear(prior), switchPriorModel));
 				}
 #endif
 				if(isSlam2d())
@@ -452,12 +614,13 @@ std::map<int, Transform> OptimizerGTSAM::optimize(
 					   iter->second.type() != Link::kNeighborMerged)
 					{
 						// 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));
+						graph.add(vertigo::BetweenFactorSwitchableLinear<gtsam::Pose2>(id1, id2, gtsam::Symbol('s', lastSwitchId_++), gtsam::Pose2(iter->second.transform().x(), iter->second.transform().y(), iter->second.transform().theta()), model));
 					}
 					else
 #endif
 					{
 						graph.add(gtsam::BetweenFactor<gtsam::Pose2>(id1, id2, gtsam::Pose2(iter->second.transform().x(), iter->second.transform().y(), iter->second.transform().theta()), model));
+						lastAddedConstraints_.push_back(ConstraintToFactor(id1, id2, -1));
 					}
 				}
 				else
@@ -481,61 +644,68 @@ std::map<int, Transform> OptimizerGTSAM::optimize(
 					   iter->second.type() != Link::kNeighborMerged)
 					{
 						// create switchable edge factor
-						graph.add(vertigo::BetweenFactorSwitchableLinear<gtsam::Pose3>(id1, id2, gtsam::Symbol('s', switchCounter++), gtsam::Pose3(iter->second.transform().toEigen4d()), model));
+						graph.add(vertigo::BetweenFactorSwitchableLinear<gtsam::Pose3>(id1, id2, gtsam::Symbol('s', lastSwitchId_++), gtsam::Pose3(iter->second.transform().toEigen4d()), model));
 					}
 					else
 #endif
 					{
 						graph.add(gtsam::BetweenFactor<gtsam::Pose3>(id1, id2, gtsam::Pose3(iter->second.transform().toEigen4d()), model));
+						lastAddedConstraints_.push_back(ConstraintToFactor(id1, id2, -1));
 					}
 				}
 			}
 		}
 
 		UDEBUG("create optimizer");
-		gtsam::NonlinearOptimizer * optimizer;
+		gtsam::NonlinearOptimizer * optimizer = 0;
 
-		if(optimizer_ == 2)
+		if(!isam2_) // Batch optimization
 		{
-			gtsam::DoglegParams parameters;
+			UDEBUG("Batch optimization...");
-			parameters.relativeErrorTol = epsilon();
+			if(internalOptimizerType_ == 2)
-			parameters.maxIterations = iterations();
+			{
-			optimizer = new gtsam::DoglegOptimizer(graph, initialEstimate, parameters);
+				gtsam::DoglegParams parameters;
-		}
+				parameters.relativeErrorTol = epsilon();
-		else if(optimizer_ == 1)
+				parameters.maxIterations = iterations();
-		{
+				optimizer = new gtsam::DoglegOptimizer(graph, initialEstimate, parameters);
-			gtsam::GaussNewtonParams parameters;
+			}
-			parameters.relativeErrorTol = epsilon();
+			else if(internalOptimizerType_ == 1)
-			parameters.maxIterations = iterations();
+			{
-			optimizer = new gtsam::GaussNewtonOptimizer(graph, initialEstimate, parameters);
+				gtsam::GaussNewtonParams parameters;
+				parameters.relativeErrorTol = epsilon();
+				parameters.maxIterations = iterations();
+				optimizer = new gtsam::GaussNewtonOptimizer(graph, initialEstimate, parameters);
+			}
+			else
+			{
+				gtsam::LevenbergMarquardtParams parameters;
+				parameters.relativeErrorTol = epsilon();
+				parameters.maxIterations = iterations();
+				optimizer = new gtsam::LevenbergMarquardtOptimizer(graph, initialEstimate, parameters);
+			}
 		}
 		else
 		{
-			gtsam::LevenbergMarquardtParams parameters;
+			UDEBUG("iSAM2 optimization...");
-			parameters.relativeErrorTol = epsilon();
-			parameters.maxIterations = iterations();
-			optimizer = new gtsam::LevenbergMarquardtOptimizer(graph, initialEstimate, parameters);
 		}
 
 		UDEBUG("GTSAM optimizing begin (max iterations=%d, robust=%d)", iterations(), isRobust()?1:0);
 		UTimer timer;
 		int it = 0;
-		double lastError = optimizer->error();
+		double initialError = optimizer?graph.error(initialEstimate):0;
+		double lastError = optimizer?optimizer->error():0;
 		for(int i=0; i<iterations(); ++i)
 		{
 			if(intermediateGraphes && i > 0)
 			{
 				float x,y,z,roll,pitch,yaw;
 				std::map<int, Transform> tmpPoses;
-#if GTSAM_VERSION_NUMERIC >= 40200
+				Values values = isam2_?isam2_->calculateEstimate():optimizer->values();
-				for(gtsam::Values::deref_iterator iter=optimizer->values().begin(); iter!=optimizer->values().end(); ++iter)
+				for(gtsam::Values::iterator iter=values.begin(); iter!=values.end(); ++iter)
-#else
-				for(gtsam::Values::const_iterator iter=optimizer->values().begin(); iter!=optimizer->values().end(); ++iter)
-#endif
 				{
-					if(iter->value.dim() > 1)
+					int key = (int)iter->key;
+					if(iter->value.dim() > 1 && uContains(newPoses, key))
 					{
-						int key = (int)iter->key;
 						if(isSlam2d())
 						{
 							if(key > 0)
@@ -547,13 +717,13 @@ std::map<int, Transform> OptimizerGTSAM::optimize(
 							{
 								if(isLandmarkWithRotation.at(key))
 								{
-									poses.at(key).getTranslationAndEulerAngles(x,y,z,roll,pitch,yaw);
+									newPoses.at(key).getTranslationAndEulerAngles(x,y,z,roll,pitch,yaw);
 									gtsam::Pose2 p = iter->value.cast<gtsam::Pose2>();
 									tmpPoses.insert(std::make_pair(key, Transform(p.x(), p.y(), z, roll, pitch, p.theta())));
 								}
 								else
 								{
-									poses.at(key).getTranslationAndEulerAngles(x,y,z,roll,pitch,yaw);
+									newPoses.at(key).getTranslationAndEulerAngles(x,y,z,roll,pitch,yaw);
 									gtsam::Point2 p = iter->value.cast<gtsam::Point2>();
 									tmpPoses.insert(std::make_pair(key, Transform(p.x(), p.y(), z, roll,pitch,yaw)));
 								}
@@ -575,7 +745,7 @@ std::map<int, Transform> OptimizerGTSAM::optimize(
 								}
 								else
 								{
-									poses.at(key).getTranslationAndEulerAngles(x,y,z,roll,pitch,yaw);
+									newPoses.at(key).getTranslationAndEulerAngles(x,y,z,roll,pitch,yaw);
 									gtsam::Point3 p = iter->value.cast<gtsam::Point3>();
 									tmpPoses.insert(std::make_pair(key, Transform(p.x(), p.y(), p.z(), roll,pitch,yaw)));
 								}
@@ -585,25 +755,89 @@ std::map<int, Transform> OptimizerGTSAM::optimize(
 				}
 				intermediateGraphes->push_back(tmpPoses);
 			}
+
+			gtsam::ISAM2Result result;
+			double error = 0;
 			try
 			{
-				optimizer->iterate();
+				if(optimizer) // Batch optimization
+				{
+					optimizer->iterate();
+					error = optimizer->error();
+				}
+				else if(i==0) // iSAM2 (add factors)
+				{
+					UDEBUG("Update iSAM with the new factors");
+					result = isam2_->update(graph, initialEstimate, removeFactorIndices);
+#if BOOST_VERSION >= 106800
+					UASSERT(result.errorBefore.has_value());
+					UASSERT(result.errorAfter.has_value());
+#else
+					UASSERT(result.errorBefore.is_initialized());
+					UASSERT(result.errorAfter.is_initialized());
+#endif
+					UDEBUG("error before = %f after=%f", result.errorBefore.value(), result.errorAfter.value());
+					initialError = lastError = result.errorBefore.value();
+					error = result.errorAfter.value();
+					if(!this->isRobust())
+					{
+						UASSERT_MSG(lastAddedConstraints_.size() == result.newFactorsIndices.size(),
+								uFormat("%ld versus %ld", lastAddedConstraints_.size(), result.newFactorsIndices.size()).c_str());
+						for(size_t j=0; j<result.newFactorsIndices.size(); ++j)
+						{
+							UDEBUG("New factor indice: %ld", result.newFactorsIndices[j]);
+							lastAddedConstraints_[j].factorIndice = result.newFactorsIndices[j];
+						}
+					}
+					if(rootId != 0 && lastRootFactorIndex_.first == 0)
+					{
+						UASSERT(result.newFactorsIndices.size()>=1);
+						lastRootFactorIndex_.first = rootId;
+						lastRootFactorIndex_.second = result.newFactorsIndices[0]; // first one should be always the root prior
+					}
+				}
+				else // iSAM2 (more iterations)
+				{
+					result = isam2_->update();
+#if BOOST_VERSION >= 106800
+					UASSERT(result.errorBefore.has_value());
+					UASSERT(result.errorAfter.has_value());
+#else
+					UASSERT(result.errorBefore.is_initialized());
+					UASSERT(result.errorAfter.is_initialized());
+#endif
+					UDEBUG("error before = %f after=%f", result.errorBefore.value(), result.errorAfter.value());
+
+					lastError = result.errorBefore.value();
+					error = result.errorAfter.value();
+				}
 				++it;
 			}
 			catch(gtsam::IndeterminantLinearSystemException & e)
 			{
 				UWARN("GTSAM exception caught: %s\n Graph has %d edges and %d vertices", e.what(),
-						(int)edgeConstraints.size(),
+						(int)newEdgeConstraints.size(),
-						(int)poses.size());
+						(int)newPoses.size());
 				delete optimizer;
+				if(isam2_)
+				{
+					// We are in bad state, cleanup
+					UDEBUG("Reset iSAM2!");
+					gtsam::ISAM2Params params = isam2_->params();
+					delete isam2_;
+					isam2_ = new gtsam::ISAM2(params);
+					addedPoses_.clear();
+					lastAddedConstraints_.clear();
+					lastRootFactorIndex_.first = 0;
+					lastSwitchId_ = 1000000000;
+				}
 				return optimizedPoses;
 			}
 
 			// early stop condition
-			double error = optimizer->error();
 			UDEBUG("iteration %d error =%f", i+1, error);
 			double errorDelta = lastError - error;
-			if(i>0 && errorDelta < this->epsilon())
+			if((isam2_ || i>0) && errorDelta < this->epsilon())
 			{
 				if(errorDelta < 0)
 				{
@@ -630,19 +864,16 @@ std::map<int, Transform> OptimizerGTSAM::optimize(
 		{
 			*iterationsDone = it;
 		}
-		UDEBUG("GTSAM optimizing end (%d iterations done, error=%f (initial=%f final=%f), time=%f s)",
+		UDEBUG("GTSAM optimizing end (%d iterations done (error initial=%f final=%f), time=%f s)",
-				optimizer->iterations(), optimizer->error(), graph.error(initialEstimate), graph.error(optimizer->values()), timer.ticks());
+				it, initialError, lastError, timer.ticks());
 
 		float x,y,z,roll,pitch,yaw;
-#if GTSAM_VERSION_NUMERIC >= 40200
+		gtsam::Values values = isam2_?isam2_->calculateEstimate():optimizer->values();
-		for(gtsam::Values::deref_iterator iter=optimizer->values().begin(); iter!=optimizer->values().end(); ++iter)
+		for(gtsam::Values::iterator iter=values.begin(); iter!=values.end(); ++iter)
-#else
-		for(gtsam::Values::const_iterator iter=optimizer->values().begin(); iter!=optimizer->values().end(); ++iter)
-#endif
 		{
-			if(iter->value.dim() > 1)
+			int key = (int)iter->key;
+			if(iter->value.dim() > 1 && uContains(poses, key))
 			{
-				int key = (int)iter->key;
 				if(isSlam2d())
 				{
 					if(key > 0)
@@ -694,10 +925,18 @@ std::map<int, Transform> OptimizerGTSAM::optimize(
 
 		// compute marginals
 		try {
-			UDEBUG("Computing marginals...");
+			UDEBUG("Computing marginals for node %d...", poses.rbegin()->first);
 			UTimer t;
-			gtsam::Marginals marginals(graph, optimizer->values());
+			gtsam::Matrix info;
-			gtsam::Matrix info = marginals.marginalCovariance(poses.rbegin()->first);
+			if(optimizer)
+			{
+				gtsam::Marginals marginals(graph, optimizer->values());
+			 	info = marginals.marginalCovariance(poses.rbegin()->first);
+			}
+			else //iSAM2
+			{
+				info = isam2_->marginalCovariance(poses.rbegin()->first);
+			}
 			UDEBUG("Computed marginals = %fs (key=%d)", t.ticks(), poses.rbegin()->first);
 			if(isSlam2d() && info.cols() == 3 && info.cols() == 3)
 			{

guilib/src/MainWindow.cpp

@@ -3429,7 +3429,8 @@ void MainWindow::updateMapCloud(
 		}
 	}
 	cv::Mat map8U;
-	if((_ui->graphicsView_graphView->isVisible() || _preferencesDialog->getGridMapShown()))
+	if((_ui->graphicsView_graphView->isVisible() && _ui->graphicsView_graphView->isGridMapVisible()) ||
+	   (_cloudViewer->isVisible() && _preferencesDialog->getGridMapShown()))
 	{
 		float xMin, yMin;
 		float resolution = _occupancyGrid->getCellSize();
@@ -3438,7 +3439,6 @@ void MainWindow::updateMapCloud(
 		if(_preferencesDialog->isOctomap2dGrid())
 		{
 			map8S = _octomap->createProjectionMap(xMin, yMin, resolution, 0, _preferencesDialog->getOctomapTreeDepth());
-
 		}
 		else
 #endif
@@ -3458,12 +3458,12 @@ void MainWindow::updateMapCloud(
 			//convert to gray scaled map
 			map8U = util3d::convertMap2Image8U(map8S);
 
-			if(_preferencesDialog->getGridMapShown())
+			if(_cloudViewer->isVisible() && _preferencesDialog->getGridMapShown())
 			{
 				float opacity = _preferencesDialog->getGridMapOpacity();
 				_cloudViewer->addOccupancyGridMap(map8U, resolution, xMin, yMin, opacity);
 			}
-			if(_ui->graphicsView_graphView->isVisible())
+			if(_ui->graphicsView_graphView->isVisible() && _ui->graphicsView_graphView->isGridMapVisible())
 			{
 				_ui->graphicsView_graphView->updateMap(map8U, resolution, xMin, yMin);
 			}

guilib/src/PreferencesDialog.cpp

@@ -1137,6 +1137,9 @@ PreferencesDialog::PreferencesDialog(QWidget * parent) :
 	_ui->doubleSpinBox_g2o_baseline->setObjectName(Parameters::kg2oBaseline().c_str());
 
 	_ui->comboBox_gtsam_optimizer->setObjectName(Parameters::kGTSAMOptimizer().c_str());
+	_ui->gtsam_incremental->setObjectName(Parameters::kGTSAMIncremental().c_str());
+	_ui->gtsam_incremental_threshold->setObjectName(Parameters::kGTSAMIncRelinearizeThreshold().c_str());
+	_ui->gtsam_incremental_skip->setObjectName(Parameters::kGTSAMIncRelinearizeSkip().c_str());
 
 	_ui->graphPlan_goalReachedRadius->setObjectName(Parameters::kRGBDGoalReachedRadius().c_str());
 	_ui->graphPlan_goalsSavedInUserData->setObjectName(Parameters::kRGBDGoalsSavedInUserData().c_str());

guilib/src/ui/preferencesDialog.ui

@@ -23285,6 +23285,16 @@ Lower the ratio -&gt; higher the precision.</string>
                          </item>
                         </widget>
                        </item>
+                       <item row="1" column="1">
+                        <widget class="QLabel" name="label_742">
+                         <property name="text">
+                          <string>iSAM2: Do graph optimization incrementally to increase optimization speed on loop closures. Note that only GaussNewton and Dogleg optimization algorithms are supported in this mode.</string>
+                         </property>
+                         <property name="wordWrap">
+                          <bool>true</bool>
+                         </property>
+                        </widget>
+                       </item>
                        <item row="0" column="1">
                         <widget class="QLabel" name="label_364">
                          <property name="text">
@@ -23295,6 +23305,65 @@ Lower the ratio -&gt; higher the precision.</string>
                          </property>
                         </widget>
                        </item>
+                       <item row="2" column="0">
+                        <widget class="QDoubleSpinBox" name="gtsam_incremental_threshold">
+                         <property name="decimals">
+                          <number>4</number>
+                         </property>
+                         <property name="minimum">
+                          <double>0.000100000000000</double>
+                         </property>
+                         <property name="maximum">
+                          <double>999.000000000000000</double>
+                         </property>
+                         <property name="singleStep">
+                          <double>0.100000000000000</double>
+                         </property>
+                         <property name="value">
+                          <double>0.100000000000000</double>
+                         </property>
+                        </widget>
+                       </item>
+                       <item row="2" column="1">
+                        <widget class="QLabel" name="label_743">
+                         <property name="text">
+                          <string>Only relinearize variables whose linear delta magnitude is greater than this threshold. See GTSAM::ISAM2 doc for more info.</string>
+                         </property>
+                         <property name="wordWrap">
+                          <bool>true</bool>
+                         </property>
+                        </widget>
+                       </item>
+                       <item row="1" column="0">
+                        <widget class="QCheckBox" name="gtsam_incremental">
+                         <property name="text">
+                          <string/>
+                         </property>
+                        </widget>
+                       </item>
+                       <item row="3" column="0">
+                        <widget class="QSpinBox" name="gtsam_incremental_skip">
+                         <property name="minimum">
+                          <number>1</number>
+                         </property>
+                         <property name="maximum">
+                          <number>10000</number>
+                         </property>
+                         <property name="value">
+                          <number>10</number>
+                         </property>
+                        </widget>
+                       </item>
+                       <item row="3" column="1">
+                        <widget class="QLabel" name="label_744">
+                         <property name="text">
+                          <string>Only relinearize any variables every X calls to ISAM2::update(). See GTSAM::ISAM2 doc for more info.</string>
+                         </property>
+                         <property name="wordWrap">
+                          <bool>true</bool>
+                         </property>
+                        </widget>
+                       </item>
                       </layout>
                      </widget>
                     </item>