isam2: support adding/removing links

corelib/include/rtabmap/core/Statistics.h

@@ -83,6 +83,7 @@ class RTABMAP_CORE_EXPORT Statistics
 	RTABMAP_STATS(Loop, Optimization_max_error_removed_from_id, );
 	RTABMAP_STATS(Loop, Optimization_max_error_removed_to_id, );
 	RTABMAP_STATS(Loop, Optimization_max_error_removed_count, );
+	RTABMAP_STATS(Loop, Optimization_factors, );
 	RTABMAP_STATS(Loop, Linear_variance,);
 	RTABMAP_STATS(Loop, Angular_variance,);
 	RTABMAP_STATS(Loop, Landmark_detected,);

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

@@ -29,6 +29,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 #define OPTIMIZERGTSAM_H_
 
 #include <rtabmap/core/Optimizer.h>
+#include <tuple>
 
 namespace gtsam {
 	class ISAM2;
@@ -58,26 +59,25 @@ class RTABMAP_CORE_EXPORT OptimizerGTSAM : public Optimizer
 			double * finalError = 0,
 			int * iterationsDone = 0);
 
+	// True when iSAM2 (incremental) backend is active.
+	bool isIncremental() const {return isam2_ != 0;}
+
+	// Number of factors we believe are live in iSAM2.
+	std::size_t getTrackedFactorsCount() const {
+		return trackedFactors_.size() + (lastRootFactorIndex_.first != 0 ? 1 : 0);
+	}
+
+	// Live factor count reported by iSAM2 itself (should match getTrackedFactorsCount()).
+	std::size_t getISAM2LiveFactorsCount() const;
+
 private:
 	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::map<int, bool> isLandmarkWithRotation_; // persists across iSAM2 incremental calls
+	std::map<std::tuple<int, int, int>, std::uint64_t> trackedFactors_; // iSAM2 tracked constraints
 	std::pair<int, std::uint64_t> lastRootFactorIndex_;
 };
 

corelib/src/Rtabmap.cpp

@@ -29,6 +29,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 #include "rtabmap/core/Version.h"
 #include "rtabmap/core/Features2d.h"
 #include "rtabmap/core/Optimizer.h"
+#include "rtabmap/core/optimizer/OptimizerGTSAM.h"
 #include "rtabmap/core/Graph.h"
 #include "rtabmap/core/Signature.h"
 
@@ -4231,6 +4232,12 @@ bool Rtabmap::process(
 			statistics_.addStatistic(Statistics::kLoopLandmark_detected_node_ref(), landmarksDetected.empty() || landmarksDetected.begin()->second.empty()?0:*landmarksDetected.begin()->second.begin());
 			statistics_.addStatistic(Statistics::kLoopVisual_inliers_mean_dist(), loopClosureVisualInliersMeanDist);
 			statistics_.addStatistic(Statistics::kLoopVisual_inliers_distribution(), loopClosureVisualInliersDistribution);
+			OptimizerGTSAM * gtsamOpt = dynamic_cast<OptimizerGTSAM*>(_graphOptimizer);
+			if(gtsamOpt != 0 && gtsamOpt->isIncremental())
+			{
+				statistics_.addStatistic(Statistics::kLoopOptimization_factors(), (float)gtsamOpt->getTrackedFactorsCount());
+				statistics_.addStatistic("Loop/Optimization_internal_factors", (float)gtsamOpt->getISAM2LiveFactorsCount());
+			}
 
 			statistics_.addStatistic(Statistics::kProximityTime_detections(), proximityDetectionsInTimeFound);
 			statistics_.addStatistic(Statistics::kProximitySpace_detections_added_visually(), proximityDetectionsAddedVisually);